From ce7f6f5850ca851d10c93302cfbc1c2440864b26 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 11 Oct 2023 15:03:01 +0800 Subject: [PATCH] Implement service record and recorded service info display Signed-off-by: Barry Xu --- docs/message_definition_encoding.md | 14 +- ros2bag/ros2bag/api/__init__.py | 23 ++- ros2bag/ros2bag/verb/info.py | 28 ++- ros2bag/ros2bag/verb/record.py | 79 +++++++-- rosbag2_cpp/CMakeLists.txt | 9 +- rosbag2_cpp/include/rosbag2_cpp/info.hpp | 14 ++ .../local_message_definition_source.hpp | 6 +- .../include/rosbag2_cpp/service_utils.hpp | 41 +++++ rosbag2_cpp/src/rosbag2_cpp/info.cpp | 125 +++++++++++++- .../local_message_definition_source.cpp | 32 ++-- rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 136 +++++++++++++++ .../rosbag2_cpp/writers/sequential_writer.cpp | 11 +- .../test_local_message_definition_source.cpp | 19 +- .../test/rosbag2_cpp/test_service_utils.cpp | 76 ++++++++ rosbag2_py/CMakeLists.txt | 12 +- rosbag2_py/src/rosbag2_py/_info.cpp | 31 +++- rosbag2_py/src/rosbag2_py/_transport.cpp | 7 +- .../src/rosbag2_py/format_bag_metadata.cpp | 119 ++++++++++++- .../src/rosbag2_py/format_bag_metadata.hpp | 3 +- .../src/rosbag2_py/format_service_info.cpp | 50 ++++++ .../src/rosbag2_py/format_service_info.hpp | 27 +++ rosbag2_py/test/test_reindexer.py | 2 +- rosbag2_py/test/test_transport.py | 4 +- .../rosbag2_test_common/client_manager.hpp | 126 ++++++++++++++ .../rosbag2_test_common/service_manager.hpp | 89 ++++++++++ rosbag2_test_msgdefs/CMakeLists.txt | 1 + rosbag2_test_msgdefs/srv/ComplexSrv.srv | 3 + .../rosbag2_transport/record_options.hpp | 7 +- .../src/rosbag2_transport/record_options.cpp | 14 +- .../src/rosbag2_transport/recorder.cpp | 8 + .../src/rosbag2_transport/topic_filter.cpp | 100 +++++++++-- .../test_keyboard_controls.cpp | 3 +- .../test/rosbag2_transport/test_record.cpp | 19 +- .../rosbag2_transport/test_record_all.cpp | 85 ++++++++- .../test_record_all_ignore_leaf_topics.cpp | 3 +- ..._record_all_include_unpublished_topics.cpp | 9 +- .../test_record_all_no_discovery.cpp | 3 +- .../test_record_all_use_sim_time.cpp | 2 +- .../rosbag2_transport/test_record_options.cpp | 11 +- .../rosbag2_transport/test_record_regex.cpp | 78 ++++++++- .../test_record_services.cpp | 2 +- .../test/rosbag2_transport/test_rewrite.cpp | 14 +- .../rosbag2_transport/test_topic_filter.cpp | 162 +++++++++++++++--- rosbag2_transport/test/srv/SimpleTest.srv | 3 + 44 files changed, 1481 insertions(+), 129 deletions(-) create mode 100644 rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp create mode 100644 rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp create mode 100644 rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp create mode 100644 rosbag2_py/src/rosbag2_py/format_service_info.cpp create mode 100644 rosbag2_py/src/rosbag2_py/format_service_info.hpp create mode 100644 rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp create mode 100644 rosbag2_test_common/include/rosbag2_test_common/service_manager.hpp create mode 100644 rosbag2_test_msgdefs/srv/ComplexSrv.srv create mode 100644 rosbag2_transport/test/srv/SimpleTest.srv diff --git a/docs/message_definition_encoding.md b/docs/message_definition_encoding.md index 17e48fdb30..7d6f5a7554 100644 --- a/docs/message_definition_encoding.md +++ b/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: diff --git a/ros2bag/ros2bag/api/__init__.py b/ros2bag/ros2bag/api/__init__.py index 0c9a192ed2..5fdcb55a8e 100644 --- a/ros2bag/ros2bag/api/__init__.py +++ b/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 diff --git a/ros2bag/ros2bag/verb/info.py b/ros2bag/ros2bag/verb/info.py index 7b78730edd..5ab1000da0 100644 --- a/ros2bag/ros2bag/verb/info.py +++ b/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) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 8c49c4b679..3b3e0ed1da 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/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() diff --git a/rosbag2_cpp/CMakeLists.txt b/rosbag2_cpp/CMakeLists.txt index 8d61f3af6d..8ad64fc2fe 100644 --- a/rosbag2_cpp/CMakeLists.txt +++ b/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() diff --git a/rosbag2_cpp/include/rosbag2_cpp/info.hpp b/rosbag2_cpp/include/rosbag2_cpp/info.hpp index c3c2b2b7d7..a251598ea5 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/info.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/info.hpp @@ -15,7 +15,9 @@ #ifndef ROSBAG2_CPP__INFO_HPP_ #define ROSBAG2_CPP__INFO_HPP_ +#include #include +#include #include "rosbag2_cpp/visibility_control.hpp" @@ -24,6 +26,15 @@ 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: @@ -31,6 +42,9 @@ class ROSBAG2_CPP_PUBLIC Info virtual rosbag2_storage::BagMetadata read_metadata( const std::string & uri, const std::string & storage_id = ""); + + virtual std::vector> read_service_info( + const std::string & uri, const std::string & storage_id = ""); }; } // namespace rosbag2_cpp diff --git a/rosbag2_cpp/include/rosbag2_cpp/message_definitions/local_message_definition_source.hpp b/rosbag2_cpp/include/rosbag2_cpp/message_definitions/local_message_definition_source.hpp index 004152cbd0..706ba9ecf7 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/message_definitions/local_message_definition_source.hpp +++ b/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; diff --git a/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp new file mode 100644 index 0000000000..188d18396c --- /dev/null +++ b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp @@ -0,0 +1,41 @@ +// 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 + +#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); + +ROSBAG2_CPP_PUBLIC +std::string +service_event_topic_name_to_service_name(const std::string & topic_name); + +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(); +} // namespace rosbag2_cpp + +#endif // ROSBAG2_CPP__SERVICE_UTILS_HPP_ diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index 4d897c7aa9..cf4faade5c 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -14,16 +14,23 @@ #include "rosbag2_cpp/info.hpp" -#include +#include +#include #include #include +#include "rcl/service_introspection.h" +#include "rmw/rmw.h" + #include "rcpputils/filesystem_helper.hpp" +#include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/logging.hpp" #include "rosbag2_storage/metadata_io.hpp" #include "rosbag2_storage/storage_interfaces/read_only_interface.hpp" #include "rosbag2_storage/storage_factory.hpp" +#include "service_msgs/msg/service_event_info.hpp" + namespace rosbag2_cpp { @@ -52,4 +59,120 @@ rosbag2_storage::BagMetadata Info::read_metadata( return storage->get_metadata(); } +namespace +{ +struct client_id_hash +{ + std::size_t operator()(const std::array & client_id) const + { + std::hash 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 = std::array; +using sequence_set = std::unordered_set; +struct service_req_resp_info +{ + std::unordered_map request; + std::unordered_map response; +}; +} // namespace + +std::vector> Info::read_service_info( + const std::string & uri, const std::string & storage_id) +{ + rosbag2_storage::StorageFactory factory; + auto storage = factory.open_read_only({uri, storage_id}); + if (!storage) { + throw std::runtime_error("No plugin detected that could open file " + uri); + } + + using service_analysis = + std::unordered_map>; + + std::unordered_map> all_service_info; + service_analysis service_process_info; + + auto all_topics_types = storage->get_all_topics_and_types(); + for (auto & t : all_topics_types) { + if (is_service_event_topic(t.name, t.type)) { + auto service_info = std::make_shared(); + service_info->name = service_event_topic_name_to_service_name(t.name); + service_info->type = service_event_topic_type_to_service_type(t.type); + service_info->serialization_format = t.serialization_format; + all_service_info.emplace(t.name, service_info); + service_process_info[t.name] = std::make_shared(); + } + } + + std::vector> ret_service_info; + + if (!all_service_info.empty()) { + auto msg = service_msgs::msg::ServiceEventInfo(); + const rosidl_message_type_support_t * type_support_info = + rosidl_typesupport_cpp:: + get_message_type_support_handle(); + + while (storage->has_next()) { + auto bag_msg = storage->read_next(); + + // Check if topic is service event topic + auto one_service_info = all_service_info.find(bag_msg->topic_name); + if (one_service_info == all_service_info.end()) { + continue; + } + + auto ret = rmw_deserialize( + bag_msg->serialized_data.get(), + type_support_info, + reinterpret_cast(&msg)); + if (ret != RMW_RET_OK) { + throw std::runtime_error( + "It failed to deserialize message from " + bag_msg->topic_name + " !"); + } + + switch (msg.event_type) { + case service_msgs::msg::ServiceEventInfo::REQUEST_SENT: + case service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED: + service_process_info[bag_msg->topic_name] + ->request[msg.client_gid].emplace(msg.sequence_number); + break; + case service_msgs::msg::ServiceEventInfo::RESPONSE_SENT: + case service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED: + service_process_info[bag_msg->topic_name] + ->response[msg.client_gid].emplace(msg.sequence_number); + break; + } + } + + for (auto & [topic_name, service_info] : service_process_info) { + size_t count = 0; + // Get the number of request from all clients + for (auto &[client_id, request_list] : service_info->request) { + count += request_list.size(); + } + all_service_info[topic_name]->request_count = count; + + count = 0; + // Get the number of response from all clients + for (auto &[client_id, response_list] : service_info->response) { + count += response_list.size(); + } + all_service_info[topic_name]->response_count = count; + } + + for (auto & [topic_name, service_info] : all_service_info) { + ret_service_info.emplace_back(std::move(service_info)); + } + } + + return ret_service_info; +} + } // namespace rosbag2_cpp diff --git a/rosbag2_cpp/src/rosbag2_cpp/message_definitions/local_message_definition_source.cpp b/rosbag2_cpp/src/rosbag2_cpp/message_definitions/local_message_definition_source.cpp index 74e04f9f01..1d8e5c5db5 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/message_definitions/local_message_definition_source.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/message_definitions/local_message_definition_source.cpp @@ -48,9 +48,9 @@ class TypenameNotUnderstoodError : public std::exception }; // Match datatype names (foo_msgs/Bar or foo_msgs/msg/Bar) -static const std::regex PACKAGE_TYPENAME_REGEX{R"(^([a-zA-Z0-9_]+)/(?:msg/)?([a-zA-Z0-9_]+)$)"}; +static const std::regex PACKAGE_TYPENAME_REGEX{R"(^([a-zA-Z0-9_]+)/(?:msg/|srv/)?([a-zA-Z0-9_]+)$)"}; -// Match field types from .msg definitions ("foo_msgs/Bar" in "foo_msgs/Bar[] bar") +// Match field types from .msg and .srv definitions ("foo_msgs/Bar" in "foo_msgs/Bar[] bar") static const std::regex MSG_FIELD_TYPE_REGEX{R"((?:^|\n)\s*([a-zA-Z0-9_/]+)(?:\[[^\]]*\])?\s+)"}; // match field types from `.idl` definitions ("foo_msgs/msg/bar" in #include ) @@ -102,6 +102,7 @@ std::set parse_definition_dependencies( { switch (format) { case LocalMessageDefinitionSource::Format::MSG: + case LocalMessageDefinitionSource::Format::SRV: return parse_msg_dependencies(text, package_context); case LocalMessageDefinitionSource::Format::IDL: return parse_idl_dependencies(text); @@ -117,6 +118,8 @@ static const char * extension_for_format(LocalMessageDefinitionSource::Format fo return ".msg"; case LocalMessageDefinitionSource::Format::IDL: return ".idl"; + case LocalMessageDefinitionSource::Format::SRV: + return ".srv"; default: throw std::runtime_error("switch is not exhaustive"); } @@ -134,6 +137,9 @@ std::string LocalMessageDefinitionSource::delimiter( case Format::IDL: result += "IDL: "; break; + case Format::SRV: + result += "SRV: "; + break; default: throw std::runtime_error("switch is not exhaustive"); } @@ -166,7 +172,9 @@ const LocalMessageDefinitionSource::MessageSpec & LocalMessageDefinitionSource:: } std::string package = match[1]; std::string share_dir = ament_index_cpp::get_package_share_directory(package); - std::ifstream file{share_dir + "/msg/" + match[2].str() + + std::string dir = definition_identifier.format() == Format::MSG || + definition_identifier.format() == Format::IDL ? "/msg/" : "/srv/"; + std::ifstream file{share_dir + dir + match[2].str() + extension_for_format(definition_identifier.format())}; if (!file.good()) { throw DefinitionNotFoundError(definition_identifier.topic_type()); @@ -183,7 +191,7 @@ const LocalMessageDefinitionSource::MessageSpec & LocalMessageDefinitionSource:: } rosbag2_storage::MessageDefinition LocalMessageDefinitionSource::get_full_text( - const std::string & root_topic_type) + const std::string & root_type) { std::unordered_set seen_deps; @@ -191,12 +199,14 @@ rosbag2_storage::MessageDefinition LocalMessageDefinitionSource::get_full_text( [&](const DefinitionIdentifier & definition_identifier, int32_t depth) { if (depth <= 0) { throw std::runtime_error{ - "Reached max recursion depth resolving definition of " + root_topic_type}; + "Reached max recursion depth resolving definition of " + root_type}; } const MessageSpec & spec = load_message_spec(definition_identifier); std::string result = spec.text; for (const auto & dep_name : spec.dependencies) { - DefinitionIdentifier dep(dep_name, definition_identifier.format()); + Format format = definition_identifier.format() == Format::SRV ? + Format::MSG : definition_identifier.format(); + DefinitionIdentifier dep(dep_name, format); bool inserted = seen_deps.insert(dep).second; if (inserted) { result += "\n"; @@ -208,14 +218,14 @@ rosbag2_storage::MessageDefinition LocalMessageDefinitionSource::get_full_text( }; std::string result; - Format format = Format::MSG; + Format format = root_type.find("/srv/") != std::string::npos ? Format::SRV : Format::MSG; int32_t max_recursion_depth = ROSBAG2_CPP_LOCAL_MESSAGE_DEFINITION_SOURCE_MAX_RECURSION_DEPTH; try { - result = append_recursive(DefinitionIdentifier(root_topic_type, format), max_recursion_depth); + result = append_recursive(DefinitionIdentifier(root_type, format), max_recursion_depth); } catch (const DefinitionNotFoundError & err) { ROSBAG2_CPP_LOG_WARN("No .msg definition for %s, falling back to IDL", err.what()); format = Format::IDL; - DefinitionIdentifier root_definition_identifier(root_topic_type, format); + DefinitionIdentifier root_definition_identifier(root_type, format); result = (delimiter(root_definition_identifier) + append_recursive(root_definition_identifier, max_recursion_depth)); } catch (const TypenameNotUnderstoodError & err) { @@ -230,6 +240,7 @@ rosbag2_storage::MessageDefinition LocalMessageDefinitionSource::get_full_text( out.encoding = "unknown"; break; case Format::MSG: + case Format::SRV: out.encoding = "ros2msg"; break; case Format::IDL: @@ -238,8 +249,9 @@ rosbag2_storage::MessageDefinition LocalMessageDefinitionSource::get_full_text( default: throw std::runtime_error("switch is not exhaustive"); } + out.encoded_message_definition = result; - out.topic_type = root_topic_type; + out.topic_type = root_type; return out; } } // namespace rosbag2_cpp diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp new file mode 100644 index 0000000000..19cecf3fd5 --- /dev/null +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -0,0 +1,136 @@ +// 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. + +#include + +#include "rcl/service_introspection.h" + +#include "rosbag2_cpp/service_utils.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/"; + +bool is_service_event_topic(const std::string & topic, const std::string & topic_type) +{ + if (topic.length() <= (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)) { + return false; + } + + std::string end_topic_name = topic.substr( + topic.length() - (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)); + + 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() <= std::strlen(service_event_topic_type_postfix)) { + return false; + } + + 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) && + (end_topic_name == RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); +} + +std::string service_event_topic_name_to_service_name(const std::string & topic_name) +{ + if (topic_name.length() <= (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)) { + return std::string(); + } + + if (topic_name.substr( + topic_name.length() - + (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)) != + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + { + return std::string(); + } + + std::string service_name = topic_name.substr( + 0, topic_name.length() - (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)); + + return service_name; +} + +std::string service_event_topic_type_to_service_type(const std::string & topic_type) +{ + if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { + return std::string(); + } + + // Should include '/srv/' in type + if (topic_type.find(service_event_topic_type_middle) == std::string::npos) { + return std::string(); + } + + if (topic_type.substr(topic_type.length() - std::strlen(service_event_topic_type_postfix)) != + service_event_topic_type_postfix) + { + return std::string(); + } + + std::string service_type = topic_type.substr( + 0, topic_type.length() - strlen(service_event_topic_type_postfix)); + + 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(); + + // 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) { + std::runtime_error("Cannot get ServiceEventInfo typesupport handle !"); + } + + auto service_event_info = + static_cast( + 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; +} + +} // namespace rosbag2_cpp diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 2bbafa689b..37d3d49b0e 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -29,6 +29,7 @@ #include "rosbag2_cpp/info.hpp" #include "rosbag2_cpp/logging.hpp" +#include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/default_storage_id.hpp" #include "rosbag2_storage/storage_options.hpp" @@ -195,7 +196,15 @@ void SequentialWriter::create_topic(const rosbag2_storage::TopicMetadata & topic return; } rosbag2_storage::MessageDefinition definition; - const std::string & topic_type = topic_with_type.type; + + std::string topic_type; + if (is_service_event_topic(topic_with_type.name, topic_with_type.type)) { + // change service event type to service type for next step to get message definition + topic_type = service_event_topic_type_to_service_type(topic_with_type.type); + } else { + topic_type = topic_with_type.type; + } + try { definition = message_definitions_.get_full_text(topic_type); } catch (DefinitionNotFoundError &) { diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_local_message_definition_source.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_local_message_definition_source.cpp index 7e7d09ae1d..526847dee6 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_local_message_definition_source.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_local_message_definition_source.cpp @@ -62,6 +62,23 @@ TEST(test_local_message_definition_source, can_find_msg_deps) "float32 c\n"); } +TEST(test_local_message_definition_source, can_find_srv_deps) +{ + LocalMessageDefinitionSource source; + auto result = source.get_full_text("rosbag2_test_msgdefs/srv/ComplexSrv"); + std::cout << result.encoded_message_definition << std::endl; + ASSERT_EQ(result.encoding, "ros2msg"); + ASSERT_EQ( + result.encoded_message_definition, + "rosbag2_test_msgdefs/BasicMsg req\n" + "---\n" + "rosbag2_test_msgdefs/BasicMsg resp\n" + "\n" + "================================================================================\n" + "MSG: rosbag2_test_msgdefs/BasicMsg\n" + "float32 c\n"); +} + TEST(test_local_message_definition_source, can_find_idl_deps) { LocalMessageDefinitionSource source; @@ -132,7 +149,7 @@ TEST(test_local_message_definition_source, no_crash_on_bad_name) rosbag2_storage::MessageDefinition result; ASSERT_NO_THROW( { - result = source.get_full_text("rosbag2_test_msgdefs/srv/BasicSrv_Request"); + result = source.get_full_text("rosbag2_test_msgdefs/idl/BasicSrv_Request"); }); ASSERT_EQ(result.encoding, "unknown"); } diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp new file mode 100644 index 0000000000..51420e20ac --- /dev/null +++ b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp @@ -0,0 +1,76 @@ +// 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. +#include + +#include +#include +#include + +#include "rosbag2_cpp/service_utils.hpp" + +using namespace ::testing; // NOLINT + +class ServiceUtilsTest : public Test +{ +}; + +TEST_F(ServiceUtilsTest, check_is_service_event_topic) +{ + std::vector, bool>> all_test_data = + { + {{"/abc/_service_event", "package/srv/xyz_Event"}, true}, + {{"/_service_event", "package/srv/xyz_Event"}, false}, + {{"/abc/service_event", "package/srv/xyz_Event"}, false}, + {{"/abc/_service_event", "package/xyz_Event"}, false}, + {{"/abc/_service_event", "package/srv/xyz"}, false} + }; + + for (const auto & test_data : all_test_data) { + EXPECT_TRUE( + rosbag2_cpp::is_service_event_topic( + std::get<0>(test_data.first), std::get<1>(test_data.first)) == test_data.second); + } +} + +TEST_F(ServiceUtilsTest, check_service_event_topic_name_to_service_name) +{ + std::vector> all_test_data = + { + {"/abc/_service_event", "/abc"}, + {"/_service_event", ""}, + {"/abc/service_event", ""} + }; + + for (const auto & test_data : all_test_data) { + EXPECT_TRUE( + rosbag2_cpp::service_event_topic_name_to_service_name(test_data.first) == test_data.second); + } +} + +TEST_F(ServiceUtilsTest, check_service_event_topic_type_to_service_type) +{ + std::vector> all_test_data = + { + {"package/srv/xyz_Event", "package/srv/xyz"}, + {"package/xyz_Event", ""}, + {"package/srv/Event", ""} + }; + + for (const auto & test_data : all_test_data) { + EXPECT_EQ( + rosbag2_cpp::service_event_topic_type_to_service_type(test_data.first), + test_data.second + ); + } +} diff --git a/rosbag2_py/CMakeLists.txt b/rosbag2_py/CMakeLists.txt index 0b71864e54..80bc823043 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -48,6 +48,14 @@ ament_target_dependencies(_compression_options PUBLIC "rosbag2_compression" ) +add_library(_format_output SHARED + src/rosbag2_py/format_bag_metadata.cpp + src/rosbag2_py/format_service_info.cpp +) +target_link_libraries(_format_output PUBLIC + rosbag2_cpp::rosbag2_cpp +) + pybind11_add_module(_reader SHARED src/rosbag2_py/_reader.cpp ) @@ -59,11 +67,11 @@ target_link_libraries(_reader PUBLIC pybind11_add_module(_storage SHARED src/rosbag2_py/_storage.cpp - src/rosbag2_py/format_bag_metadata.cpp ) target_link_libraries(_storage PUBLIC rosbag2_cpp::rosbag2_cpp rosbag2_storage::rosbag2_storage + _format_output ) pybind11_add_module(_writer SHARED @@ -81,6 +89,7 @@ pybind11_add_module(_info SHARED target_link_libraries(_info PUBLIC rosbag2_cpp::rosbag2_cpp rosbag2_storage::rosbag2_storage + _format_output ) pybind11_add_module(_transport SHARED @@ -105,6 +114,7 @@ target_link_libraries(_reindexer PUBLIC install( TARGETS _compression_options + _format_output _reader _storage _writer diff --git a/rosbag2_py/src/rosbag2_py/_info.cpp b/rosbag2_py/src/rosbag2_py/_info.cpp index fdd7d00dc8..f6e0f428a1 100644 --- a/rosbag2_py/src/rosbag2_py/_info.cpp +++ b/rosbag2_py/src/rosbag2_py/_info.cpp @@ -15,6 +15,8 @@ #include #include +#include "format_bag_metadata.hpp" +#include "format_service_info.hpp" #include "rosbag2_cpp/info.hpp" #include "rosbag2_storage/bag_metadata.hpp" @@ -38,6 +40,30 @@ class Info return info_->read_metadata(uri, storage_id); } + void read_metadata_and_output_service_verbose( + const std::string & uri, + const std::string & storage_id) + { + auto metadata_info = read_metadata(uri, storage_id); + + std::vector> all_services_info; + for (auto & file_info : metadata_info.files) { + auto services_info = info_->read_service_info( + uri + "/" + file_info.path, + metadata_info.storage_identifier); + if (!services_info.empty()) { + all_services_info.insert( + all_services_info.end(), + services_info.begin(), + services_info.end()); + } + } + + // Output formatted metadata and service info + std::cout << format_bag_meta_data(metadata_info, true); + std::cout << format_service_info(all_services_info) << std::endl; + } + protected: std::unique_ptr info_; }; @@ -49,5 +75,8 @@ PYBIND11_MODULE(_info, m) { pybind11::class_(m, "Info") .def(pybind11::init()) - .def("read_metadata", &rosbag2_py::Info::read_metadata); + .def("read_metadata", &rosbag2_py::Info::read_metadata) + .def( + "read_metadata_and_output_service_verbose", + &rosbag2_py::Info::read_metadata_and_output_service_verbose); } diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 0237bf5cfa..d9ee4c3833 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -353,13 +353,14 @@ PYBIND11_MODULE(_transport, m) { py::class_(m, "RecordOptions") .def(py::init<>()) - .def_readwrite("all", &RecordOptions::all) + .def_readwrite("all_topics", &RecordOptions::all_topics) .def_readwrite("is_discovery_disabled", &RecordOptions::is_discovery_disabled) .def_readwrite("topics", &RecordOptions::topics) .def_readwrite("rmw_serialization_format", &RecordOptions::rmw_serialization_format) .def_readwrite("topic_polling_interval", &RecordOptions::topic_polling_interval) .def_readwrite("regex", &RecordOptions::regex) - .def_readwrite("exclude", &RecordOptions::exclude) + .def_readwrite("exclude_topics", &RecordOptions::exclude_topics) + .def_readwrite("exclude_services", &RecordOptions::exclude_services) .def_readwrite("node_prefix", &RecordOptions::node_prefix) .def_readwrite("compression_mode", &RecordOptions::compression_mode) .def_readwrite("compression_format", &RecordOptions::compression_format) @@ -374,6 +375,8 @@ PYBIND11_MODULE(_transport, m) { .def_readwrite("start_paused", &RecordOptions::start_paused) .def_readwrite("ignore_leaf_topics", &RecordOptions::ignore_leaf_topics) .def_readwrite("use_sim_time", &RecordOptions::use_sim_time) + .def_readwrite("services", &RecordOptions::services) + .def_readwrite("all_services", &RecordOptions::all_services) ; py::class_(m, "Player") diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp index fc8408db45..a2189bb1fa 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "format_bag_metadata.hpp" - #include #include #include +#include #include #include #include @@ -26,8 +25,11 @@ #include #endif +#include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/bag_metadata.hpp" +#include "format_bag_metadata.hpp" + namespace { @@ -124,17 +126,105 @@ void format_topics_with_type( info_stream << std::endl; }; - print_topic_info(topics[0]); size_t number_of_topics = topics.size(); - for (size_t j = 1; j < number_of_topics; ++j) { + size_t i = 0; + // Find first topic which isn't service event topic + while (i < number_of_topics && + rosbag2_cpp::is_service_event_topic( + topics[i].topic_metadata.name, + topics[i].topic_metadata.type)) + { + i++; + } + + if (i == number_of_topics) { + return; + } + + print_topic_info(topics[i]); + for (size_t j = ++i; j < number_of_topics; ++j) { + if (rosbag2_cpp::is_service_event_topic( + topics[j].topic_metadata.name, topics[j].topic_metadata.type)) + { + continue; + } indent(info_stream, indentation_spaces); print_topic_info(topics[j]); } } +struct ServiceMetadata +{ + std::string name; + std::string type; + std::string serialization_format; +}; + +struct ServiceInformation +{ + ServiceMetadata service_metadata; + size_t event_message_count; +}; + +std::vector> filter_service_event_topic( + const std::vector & topics_with_message_count, + size_t & total_service_event_msg_count) +{ + total_service_event_msg_count = 0; + std::vector> service_info_list; + + for (auto & topic : topics_with_message_count) { + if (rosbag2_cpp::is_service_event_topic( + topic.topic_metadata.name, topic.topic_metadata.type)) + { + auto service_info = std::make_shared(); + service_info->service_metadata.name = + rosbag2_cpp::service_event_topic_name_to_service_name(topic.topic_metadata.name); + service_info->service_metadata.type = + rosbag2_cpp::service_event_topic_type_to_service_type(topic.topic_metadata.type); + service_info->service_metadata.serialization_format = + topic.topic_metadata.serialization_format; + service_info->event_message_count = topic.message_count; + total_service_event_msg_count += topic.message_count; + service_info_list.emplace_back(service_info); + } + } + + return service_info_list; +} + +void format_service_with_type( + const std::vector> & services, + std::stringstream & info_stream, + int indentation_spaces) +{ + if (services.empty()) { + info_stream << std::endl; + return; + } + + auto print_service_info = + [&info_stream](const std::shared_ptr & si) -> void { + info_stream << "Service: " << si->service_metadata.name << " | "; + info_stream << "Type: " << si->service_metadata.type << " | "; + info_stream << "Event Count: " << si->event_message_count << " | "; + info_stream << "Serialization Format: " << si->service_metadata.serialization_format; + info_stream << std::endl; + }; + + print_service_info(services[0]); + auto number_of_services = services.size(); + for (size_t j = 1; j < number_of_services; ++j) { + indent(info_stream, indentation_spaces); + print_service_info(services[j]); + } +} + } // namespace -std::string format_bag_meta_data(const rosbag2_storage::BagMetadata & metadata) +std::string format_bag_meta_data( + const rosbag2_storage::BagMetadata & metadata, + bool only_topic) { auto start_time = metadata.starting_time.time_since_epoch(); auto end_time = start_time + metadata.duration; @@ -145,6 +235,14 @@ std::string format_bag_meta_data(const rosbag2_storage::BagMetadata & metadata) ros_distro = "unknown"; } + size_t total_service_event_msg_count = 0; + std::vector> service_info_list; + if (!only_topic) { + service_info_list = filter_service_event_topic( + metadata.topics_with_message_count, + total_service_event_msg_count); + } + info_stream << std::endl; info_stream << "Files: "; format_file_paths(metadata.relative_file_paths, info_stream, indentation_spaces); @@ -157,10 +255,19 @@ std::string format_bag_meta_data(const rosbag2_storage::BagMetadata & metadata) info_stream << "Start: " << format_time_point(start_time) << std::endl; info_stream << "End: " << format_time_point(end_time) << std::endl; - info_stream << "Messages: " << metadata.message_count << std::endl; + info_stream << "Messages: " << metadata.message_count - total_service_event_msg_count << + std::endl; info_stream << "Topic information: "; format_topics_with_type( metadata.topics_with_message_count, info_stream, indentation_spaces); + if (!only_topic) { + info_stream << "Service: " << service_info_list.size() << std::endl; + info_stream << "Service information: "; + if (service_info_list.size() != 0) { + format_service_with_type(service_info_list, info_stream, indentation_spaces + 2); + } + } + return info_stream.str(); } diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp index 30cd8d4344..ba2e868ed7 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp @@ -19,6 +19,7 @@ #include "rosbag2_storage/bag_metadata.hpp" -std::string format_bag_meta_data(const rosbag2_storage::BagMetadata & metadata); +std::string format_bag_meta_data( + const rosbag2_storage::BagMetadata & metadata, bool only_topic = false); #endif // ROSBAG2_PY__FORMAT_BAG_METADATA_HPP_ diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.cpp b/rosbag2_py/src/rosbag2_py/format_service_info.cpp new file mode 100644 index 0000000000..0c1366bee9 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/format_service_info.cpp @@ -0,0 +1,50 @@ +// 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. + +#include + +#include "format_service_info.hpp" + +std::string +format_service_info( + std::vector> & service_info_list) +{ + std::stringstream info_stream; + int indentation_spaces = 21; + info_stream << "Service: " << service_info_list.size() << std::endl; + info_stream << "Service information: "; + + if (service_info_list.empty()) { + return info_stream.str(); + } + + auto print_service_info = + [&info_stream](const std::shared_ptr & si) -> void { + info_stream << "Service: " << si->name << " | "; + info_stream << "Type: " << si->type << " | "; + info_stream << "Request Count: " << si->request_count << " | "; + info_stream << "Response Count: " << si->response_count << " | "; + info_stream << "Serialization Format: " << si->serialization_format; + info_stream << std::endl; + }; + + print_service_info(service_info_list[0]); + auto number_of_services = service_info_list.size(); + for (size_t j = 1; j < number_of_services; ++j) { + info_stream << std::string(indentation_spaces, ' '); + print_service_info(service_info_list[j]); + } + + return info_stream.str(); +} diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.hpp b/rosbag2_py/src/rosbag2_py/format_service_info.hpp new file mode 100644 index 0000000000..7337e5be4e --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/format_service_info.hpp @@ -0,0 +1,27 @@ +// 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_PY__FORMAT_SERVICE_INFO_HPP_ +#define ROSBAG2_PY__FORMAT_SERVICE_INFO_HPP_ + +#include +#include +#include + +#include "rosbag2_cpp/info.hpp" + +std::string format_service_info( + std::vector> & service_info); + +#endif // ROSBAG2_PY__FORMAT_SERVICE_INFO_HPP_ diff --git a/rosbag2_py/test/test_reindexer.py b/rosbag2_py/test/test_reindexer.py index caa9375ffb..5369feb996 100644 --- a/rosbag2_py/test/test_reindexer.py +++ b/rosbag2_py/test/test_reindexer.py @@ -43,7 +43,7 @@ def test_reindexer_multiple_files(storage_id): reindexer = rosbag2_py.Reindexer() reindexer.reindex(storage_options) - assert(result_path.exists()) + assert result_path.exists() try: result_path.unlink() diff --git a/rosbag2_py/test/test_transport.py b/rosbag2_py/test/test_transport.py index 548708d409..fe01d28b93 100644 --- a/rosbag2_py/test/test_transport.py +++ b/rosbag2_py/test/test_transport.py @@ -50,7 +50,7 @@ def test_record_cancel(tmp_path, storage_id): recorder = rosbag2_py.Recorder() record_options = rosbag2_py.RecordOptions() - record_options.all = True + record_options.all_topics = True record_options.is_discovery_disabled = False record_options.topic_polling_interval = datetime.timedelta(milliseconds=100) @@ -83,7 +83,7 @@ def test_record_cancel(tmp_path, storage_id): record_thread.join() metadata = metadata_io.read_metadata(bag_path) - assert(len(metadata.relative_file_paths)) + assert len(metadata.relative_file_paths) storage_path = Path(metadata.relative_file_paths[0]) assert wait_for(lambda: storage_path.is_file(), timeout=rclpy.duration.Duration(seconds=3)) diff --git a/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp new file mode 100644 index 0000000000..a7764b4e49 --- /dev/null +++ b/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp @@ -0,0 +1,126 @@ +// 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_TEST_COMMON__CLIENT_MANAGER_HPP_ +#define ROSBAG2_TEST_COMMON__CLIENT_MANAGER_HPP_ + +#include +#include +#include + +#include "rcl/service_introspection.h" + +#include "rclcpp/rclcpp.hpp" // rclcpp must be included before the Windows specific includes. + + +namespace rosbag2_test_common +{ +template +class ClientManager : public rclcpp::Node +{ +public: + explicit ClientManager( + std::string service_name, + size_t client_number = 1, + bool service_event_contents = false, + bool client_event_contents = true) + : Node("service_client_manager_" + std::to_string(rclcpp::Clock().now().nanoseconds()), + rclcpp::NodeOptions().start_parameter_services(false).start_parameter_event_publisher( + false).enable_rosout(false)), + service_name_(service_name), + client_number_(client_number), + enable_service_event_contents_(service_event_contents), + enable_client_event_contents_(client_event_contents) + { + auto echo_process = + [this](const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) -> void + { + // Do nothing + (void)request_header; + (void)request; + (void)response; + }; + + service_ = create_service(service_name_, echo_process); + + rcl_service_introspection_state_t introspection_state; + if (enable_service_event_contents_) { + introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; + } else { + introspection_state = RCL_SERVICE_INTROSPECTION_OFF; + } + service_->configure_introspection( + get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state); + + if (enable_client_event_contents_) { + introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; + } else { + introspection_state = RCL_SERVICE_INTROSPECTION_OFF; + } + + for (size_t i = 0; i < client_number_; i++) { + auto client = create_client(service_name_); + client->configure_introspection( + get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state); + clients_.emplace_back(client); + } + } + + bool check_service_ready() + { + for (auto & client : clients_) { + if (!client->service_is_ready()) { + return false; + } + } + return true; + } + + bool send_request() + { + if (!check_service_ready()) { + return false; + } + + for (auto & client : clients_) { + auto request = std::make_shared(); + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::executors::spin_node_until_future_complete( + exec_, get_node_base_interface(), result) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO( + rclcpp::get_logger("service_client_manager"), "Failed to get response !"); + return false; + } + } + return true; + } + + using client_shared_ptr = typename rclcpp::Client::SharedPtr; + +private: + rclcpp::executors::SingleThreadedExecutor exec_; + typename rclcpp::Service::SharedPtr service_; + std::vector clients_; + const std::string service_name_; + size_t client_number_; + bool enable_service_event_contents_; + bool enable_client_event_contents_; +}; +} // namespace rosbag2_test_common + +#endif // ROSBAG2_TEST_COMMON__CLIENT_MANAGER_HPP_ diff --git a/rosbag2_test_common/include/rosbag2_test_common/service_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/service_manager.hpp new file mode 100644 index 0000000000..ea640eccb6 --- /dev/null +++ b/rosbag2_test_common/include/rosbag2_test_common/service_manager.hpp @@ -0,0 +1,89 @@ +// 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_TEST_COMMON__SERVICE_MANAGER_HPP_ +#define ROSBAG2_TEST_COMMON__SERVICE_MANAGER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +namespace rosbag2_test_common +{ +class ServiceManager +{ +public: + ServiceManager() + : pub_node_(std::make_shared( + "service_manager_" + std::to_string(rclcpp::Clock().now().nanoseconds()), + rclcpp::NodeOptions().start_parameter_event_publisher(false).enable_rosout(false))) + { + } + + ~ServiceManager() + { + exit_ = true; + if (thread_.joinable()) { + thread_.join(); + } + } + + template + void setup_service( + std::string service_name, + std::vector> & requests) + { + auto callback = [&requests]( + const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) { + (void)request_header; + (void)response; + requests.emplace_back(request); + }; + + auto service = pub_node_->create_service( + service_name, std::forward(callback)); + services_.emplace(service_name, service); + } + + void run_services() + { + auto spin = [this]() { + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(pub_node_); + + while (!exit_) { + exec.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + }; + + thread_ = std::thread(spin); + } + +private: + std::shared_ptr pub_node_; + std::unordered_map services_; + bool exit_ = false; + std::thread thread_; +}; + +} // namespace rosbag2_test_common + +#endif // ROSBAG2_TEST_COMMON__SERVICE_MANAGER_HPP_ diff --git a/rosbag2_test_msgdefs/CMakeLists.txt b/rosbag2_test_msgdefs/CMakeLists.txt index 32199bf446..f50036ab08 100644 --- a/rosbag2_test_msgdefs/CMakeLists.txt +++ b/rosbag2_test_msgdefs/CMakeLists.txt @@ -15,6 +15,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ComplexMsg.msg" "msg/ComplexMsgDependsOnIdl.msg" "srv/BasicSrv.srv" + "srv/ComplexSrv.srv" ADD_LINTER_TESTS ) diff --git a/rosbag2_test_msgdefs/srv/ComplexSrv.srv b/rosbag2_test_msgdefs/srv/ComplexSrv.srv new file mode 100644 index 0000000000..d987d100fd --- /dev/null +++ b/rosbag2_test_msgdefs/srv/ComplexSrv.srv @@ -0,0 +1,3 @@ +rosbag2_test_msgdefs/BasicMsg req +--- +rosbag2_test_msgdefs/BasicMsg resp diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index a387ab0998..385187133a 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -30,13 +30,16 @@ namespace rosbag2_transport struct RecordOptions { public: - bool all = false; + bool all_topics = false; + bool all_services = false; bool is_discovery_disabled = false; std::vector topics; + std::vector services; // service event topic std::string rmw_serialization_format; std::chrono::milliseconds topic_polling_interval{100}; std::string regex = ""; - std::string exclude = ""; + std::string exclude_topics = ""; + std::string exclude_services = ""; std::string node_prefix = ""; std::string compression_mode = ""; std::string compression_format = ""; diff --git a/rosbag2_transport/src/rosbag2_transport/record_options.cpp b/rosbag2_transport/src/rosbag2_transport/record_options.cpp index 9f94ac68a3..c8b8519a24 100644 --- a/rosbag2_transport/src/rosbag2_transport/record_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/record_options.cpp @@ -42,13 +42,16 @@ Node convert::encode( const rosbag2_transport::RecordOptions & record_options) { Node node; - node["all"] = record_options.all; + node["all_topics"] = record_options.all_topics; + node["all_services"] = record_options.all_services; node["is_discovery_disabled"] = record_options.is_discovery_disabled; node["topics"] = record_options.topics; + node["services"] = record_options.services; node["rmw_serialization_format"] = record_options.rmw_serialization_format; node["topic_polling_interval"] = record_options.topic_polling_interval; node["regex"] = record_options.regex; - node["exclude"] = record_options.exclude; + node["exclude_topics"] = record_options.exclude_topics; + node["exclude_services"] = record_options.exclude_services; node["node_prefix"] = record_options.node_prefix; node["compression_mode"] = record_options.compression_mode; node["compression_format"] = record_options.compression_format; @@ -66,15 +69,18 @@ Node convert::encode( bool convert::decode( const Node & node, rosbag2_transport::RecordOptions & record_options) { - optional_assign(node, "all", record_options.all); + optional_assign(node, "all_topics", record_options.all_topics); + optional_assign(node, "all_services", record_options.all_services); optional_assign(node, "is_discovery_disabled", record_options.is_discovery_disabled); optional_assign>(node, "topics", record_options.topics); + optional_assign>(node, "services", record_options.services); optional_assign( node, "rmw_serialization_format", record_options.rmw_serialization_format); optional_assign( node, "topic_polling_interval", record_options.topic_polling_interval); optional_assign(node, "regex", record_options.regex); - optional_assign(node, "exclude", record_options.exclude); + optional_assign(node, "exclude_topics", record_options.exclude_topics); + optional_assign(node, "exclude_services", record_options.exclude_services); optional_assign(node, "node_prefix", record_options.node_prefix); optional_assign(node, "compression_mode", record_options.compression_mode); optional_assign(node, "compression_format", record_options.compression_format); diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 69c280e136..dac7496c1f 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -31,6 +31,7 @@ #include "rosbag2_cpp/bag_events.hpp" #include "rosbag2_cpp/writer.hpp" +#include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_interfaces/srv/snapshot.hpp" @@ -475,6 +476,13 @@ void RecorderImpl::subscribe_topic(const rosbag2_storage::TopicMetadata & topic) writer_->create_topic(topic); Rosbag2QoS subscription_qos{subscription_qos_for_topic(topic.name)}; + + // For service event topic, avoid receiving the last response message. + // TODO(Barry-Xu-2018): is there a better way ? + if (rosbag2_cpp::is_service_event_topic(topic.name, topic.type)) { + subscription_qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + auto subscription = create_subscription(topic.name, topic.type, subscription_qos); if (subscription) { subscriptions_.insert({topic.name, subscription}); diff --git a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp index a09f6b70c3..aee426d447 100644 --- a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp +++ b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp @@ -23,6 +23,7 @@ #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rcpputils/split.hpp" #include "rosbag2_cpp/typesupport_helpers.hpp" +#include "rosbag2_cpp/service_utils.hpp" #include "logging.hpp" #include "rosbag2_transport/topic_filter.hpp" @@ -70,6 +71,18 @@ bool topic_in_list(const std::string & topic_name, const std::vector & service_event_topics) +{ + size_t pos = topic_name.rfind(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); + if (pos == std::string::npos) { + return false; + } + auto it = std::find(service_event_topics.begin(), service_event_topics.end(), topic_name); + return it != service_event_topics.end(); +} + bool topic_is_unpublished( const std::string & topic_name, rclcpp::node_interfaces::NodeGraphInterface & node_graph) @@ -117,6 +130,13 @@ std::unordered_map TopicFilter::filter_topics( bool TopicFilter::take_topic( const std::string & topic_name, const std::vector & topic_types) { + if (!has_single_type(topic_name, topic_types)) { + return false; + } + + const std::string & topic_type = topic_types[0]; + bool is_service_event_topic = rosbag2_cpp::is_service_event_topic(topic_name, topic_type); + if (!record_options_.include_unpublished_topics && node_graph_ && topic_is_unpublished(topic_name, *node_graph_)) { @@ -129,36 +149,78 @@ bool TopicFilter::take_topic( return false; } - if (!record_options_.topics.empty() && !topic_in_list(topic_name, record_options_.topics)) { - return false; - } + if (!is_service_event_topic) { + if (!record_options_.all_topics && + record_options_.topics.empty() && + record_options_.regex.empty() && + !record_options_.include_hidden_topics) + { + return false; + } - std::regex exclude_regex(record_options_.exclude); - if (!record_options_.exclude.empty() && std::regex_search(topic_name, exclude_regex)) { - return false; - } + if (!record_options_.topics.empty() && !topic_in_list(topic_name, record_options_.topics)) { + return false; + } - std::regex include_regex(record_options_.regex); - if ( - !record_options_.all && // All takes precedence over regex - !record_options_.regex.empty() && // empty regex matches nothing, but should be ignored - !std::regex_search(topic_name, include_regex)) - { - return false; - } + std::regex exclude_topics_regex(record_options_.exclude_topics); + if (!record_options_.exclude_topics.empty() && + std::regex_search(topic_name, exclude_topics_regex)) + { + return false; + } - if (!has_single_type(topic_name, topic_types)) { - return false; + std::regex include_regex(record_options_.regex); + if (!record_options_.all_topics && // All takes precedence over regex + !record_options_.regex.empty() && // empty regex matches nothing, but should be ignored + !std::regex_search(topic_name, include_regex)) + { + return false; + } + } else { + if (!record_options_.all_services && + record_options_.services.empty() && + record_options_.regex.empty()) + { + return false; + } + + if (!record_options_.services.empty() && + !service_in_list(topic_name, record_options_.services)) + { + return false; + } + + // Convert service event topic name to service name + auto service_name = + rosbag2_cpp::service_event_topic_name_to_service_name(topic_name); + + std::regex exclude_services_regex(record_options_.exclude_services); + if (!record_options_.exclude_services.empty() && + std::regex_search(service_name, exclude_services_regex)) + { + return false; + } + + std::regex include_regex(record_options_.regex); + if ( + !record_options_.all_services && // All takes precedence over regex + !record_options_.regex.empty() && // empty regex matches nothing, but should be ignored + !std::regex_search(service_name, include_regex)) + { + return false; + } } - if (!record_options_.include_hidden_topics && topic_is_hidden(topic_name)) { + if (!record_options_.include_hidden_topics && + topic_is_hidden(topic_name) && + !is_service_event_topic) + { RCUTILS_LOG_WARN_ONCE_NAMED( ROSBAG2_TRANSPORT_PACKAGE_NAME, "Hidden topics are not recorded. Enable them with --include-hidden-topics"); return false; } - const std::string & topic_type = topic_types[0]; if (!allow_unknown_types_ && !type_is_known(topic_name, topic_type)) { return false; } diff --git a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp index 8e91d01763..d012075808 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp @@ -186,7 +186,8 @@ TEST_F(RecordIntegrationTestFixture, test_keyboard_controls) auto writer = std::make_unique(std::move(mock_sequential_writer)); auto keyboard_handler = std::make_shared(); - rosbag2_transport::RecordOptions record_options = {true, false, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, false, {}, {}, "rmw_format", 100ms}; record_options.start_paused = true; auto recorder = std::make_shared( diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index baff0b1583..59829f6754 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -23,6 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "rosbag2_test_common/publication_manager.hpp" +#include "rosbag2_test_common/client_manager.hpp" #include "rosbag2_test_common/wait_for.hpp" #include "rosbag2_transport/recorder.hpp" @@ -46,7 +47,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are pub_manager.setup_publisher(string_topic, string_message, 2); rosbag2_transport::RecordOptions record_options = - {false, false, {string_topic, array_topic}, "rmw_format", 50ms}; + {false, false, false, {string_topic, array_topic}, {}, "rmw_format", 50ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -97,7 +98,7 @@ TEST_F(RecordIntegrationTestFixture, can_record_again_after_stop) pub_manager.setup_publisher(string_topic, string_message, 2); rosbag2_transport::RecordOptions record_options = - {false, false, {string_topic}, "rmw_format", 50ms}; + {false, false, false, {string_topic}, {}, "rmw_format", 50ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -149,7 +150,7 @@ TEST_F(RecordIntegrationTestFixture, qos_is_stored_in_metadata) pub_manager.setup_publisher(topic, string_message, 2); rosbag2_transport::RecordOptions record_options = - {false, false, {topic}, "rmw_format", 100ms}; + {false, false, false, {topic}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -211,7 +212,7 @@ TEST_F(RecordIntegrationTestFixture, records_sensor_data) pub_manager.setup_publisher(topic, string_message, 2, rclcpp::SensorDataQoS()); rosbag2_transport::RecordOptions record_options = - {false, false, {topic}, "rmw_format", 100ms}; + {false, false, false, {topic}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -254,7 +255,7 @@ TEST_F(RecordIntegrationTestFixture, receives_latched_messages) pub_manager.run_publishers(); rosbag2_transport::RecordOptions record_options = - {false, false, {topic}, "rmw_format", 100ms}; + {false, false, false, {topic}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -298,7 +299,8 @@ TEST_F(RecordIntegrationTestFixture, mixed_qos_subscribes) { auto publisher_transient_local = publisher_node->create_publisher( topic, profile_transient_local); - rosbag2_transport::RecordOptions record_options = {false, false, {topic}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {false, false, false, {topic}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -344,7 +346,8 @@ TEST_F(RecordIntegrationTestFixture, duration_and_noncompatibility_policies_mixe .liveliness(liveliness).liveliness_lease_duration(liveliness_lease_duration); auto publisher_liveliness = create_pub(profile_liveliness); - rosbag2_transport::RecordOptions record_options = {false, false, {topic}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {false, false, false, {topic}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -382,7 +385,7 @@ TEST_F(RecordIntegrationTestFixture, write_split_callback_is_called) mock_writer.set_max_messages_per_file(5); rosbag2_transport::RecordOptions record_options = - {false, false, {string_topic}, "rmw_format", 100ms}; + {false, false, false, {string_topic}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp index 5c72b4c6e4..be76ac4ce9 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp @@ -22,9 +22,11 @@ #include "test_msgs/msg/arrays.hpp" #include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" +#include "test_msgs/srv/basic_types.hpp" #include "rosbag2_test_common/publication_manager.hpp" #include "rosbag2_test_common/wait_for.hpp" +#include "rosbag2_test_common/client_manager.hpp" #include "rosbag2_transport/recorder.hpp" @@ -47,7 +49,8 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are pub_manager.setup_publisher(array_topic, array_message, 2); pub_manager.setup_publisher(string_topic, string_message, 2); - rosbag2_transport::RecordOptions record_options = {true, false, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, false, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -85,3 +88,83 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are EXPECT_THAT(array_messages[0]->bool_values, ElementsAre(true, false, true)); EXPECT_THAT(array_messages[0]->float32_values, ElementsAre(40.0f, 2.0f, 0.0f)); } + +TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_services_are_recorded) +{ + auto client_manager_1 = + std::make_shared>( + "test_service_1"); + + auto client_manager_2 = + std::make_shared>( + "test_service_2"); + + rosbag2_transport::RecordOptions record_options = + {false, true, false, {}, {}, "rmw_format", 100ms}; + auto recorder = std::make_shared( + std::move(writer_), storage_options_, record_options); + recorder->record(); + + start_async_spin(recorder); + + ASSERT_TRUE(client_manager_1->check_service_ready()); + + ASSERT_TRUE(client_manager_2->check_service_ready()); + + // By default, only client introspection is enable. + // For one request, service event topic get 2 messages. + ASSERT_TRUE(client_manager_1->send_request()); + ASSERT_TRUE(client_manager_2->send_request()); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + auto & writer = recorder->get_writer_handle(); + MockSequentialWriter & mock_writer = + static_cast(writer.get_implementation_handle()); + + size_t expected_messages = 4; + auto recorded_messages = mock_writer.get_messages(); + EXPECT_EQ(recorded_messages.size(), expected_messages); +} + +TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_are_recorded) +{ + auto client_manager_1 = + std::make_shared>( + "test_service"); + + auto string_message = get_messages_strings()[0]; + string_message->string_value = "Hello World"; + std::string string_topic = "/string_topic"; + rosbag2_test_common::PublicationManager pub_manager; + pub_manager.setup_publisher(string_topic, string_message, 1); + + rosbag2_transport::RecordOptions record_options = + {true, true, false, {}, {}, "rmw_format", 100ms}; + record_options.exclude_topics = "rosout"; + auto recorder = std::make_shared( + std::move(writer_), storage_options_, record_options); + recorder->record(); + + start_async_spin(recorder); + + ASSERT_TRUE(pub_manager.wait_for_matched(string_topic.c_str())); + + ASSERT_TRUE(client_manager_1->check_service_ready()); + + pub_manager.run_publishers(); + + // By default, only client introspection is enable. + // For one request, service event topic get 2 messages. + ASSERT_TRUE(client_manager_1->send_request()); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + auto & writer = recorder->get_writer_handle(); + MockSequentialWriter & mock_writer = + static_cast(writer.get_implementation_handle()); + + size_t expected_messages = 3; + auto recorded_messages = mock_writer.get_messages(); + EXPECT_EQ(recorded_messages.size(), expected_messages); +} diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp index 977aacfe6d..292cfbdd86 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp @@ -51,7 +51,8 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_two_topics_ignore_l pub_manager.setup_publisher(array_topic, array_message, 2); pub_manager.setup_publisher(string_topic, string_message, 2); - rosbag2_transport::RecordOptions record_options = {true, false, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, false, {}, {}, "rmw_format", 100ms}; record_options.ignore_leaf_topics = true; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all_include_unpublished_topics.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_include_unpublished_topics.cpp index 809a1c6ad5..644840d976 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all_include_unpublished_topics.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all_include_unpublished_topics.cpp @@ -34,7 +34,8 @@ TEST_F(RecordIntegrationTestFixture, record_all_include_unpublished_false_ignore auto string_msgs_sub = node->create_subscription( string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {}); - rosbag2_transport::RecordOptions record_options = {true, false, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, false, {}, {}, "rmw_format", 100ms}; record_options.include_unpublished_topics = false; auto recorder = std::make_shared(writer_, storage_options_, record_options); recorder->record(); @@ -51,7 +52,8 @@ TEST_F(RecordIntegrationTestFixture, record_all_include_unpublished_true_include auto string_msgs_sub = node->create_subscription( string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {}); - rosbag2_transport::RecordOptions record_options = {true, false, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, false, {}, {}, "rmw_format", 100ms}; record_options.include_unpublished_topics = true; auto recorder = std::make_shared(writer_, storage_options_, record_options); recorder->record(); @@ -70,7 +72,8 @@ TEST_F( auto string_msgs_sub = node->create_subscription( string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {}); - rosbag2_transport::RecordOptions record_options = {true, false, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, false, {}, {}, "rmw_format", 100ms}; record_options.include_unpublished_topics = false; auto recorder = std::make_shared(writer_, storage_options_, record_options); recorder->record(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp index e4aee9171b..845206cbeb 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp @@ -37,7 +37,8 @@ TEST_F(RecordIntegrationTestFixture, record_all_without_discovery_ignores_later_ auto string_message = get_messages_strings()[0]; string_message->string_value = "Hello World"; - rosbag2_transport::RecordOptions record_options = {true, true, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, true, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp index f512307219..a2b9a9f192 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp @@ -91,7 +91,7 @@ TEST_F(RecordIntegrationTestFixture, record_all_with_sim_time) rosbag2_transport::RecordOptions record_options = { - false, false, {string_topic, clock_topic}, "rmw_format", 100ms + false, false, false, {string_topic, clock_topic}, {}, "rmw_format", 100ms }; record_options.use_sim_time = true; auto recorder = std::make_shared( diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp index e305229152..157aeda063 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp @@ -21,13 +21,16 @@ using namespace ::testing; // NOLINT TEST(record_options, test_yaml_serialization) { rosbag2_transport::RecordOptions original; - original.all = true; + original.all_topics = true; + original.all_services = true; original.is_discovery_disabled = true; original.topics = {"topic", "other_topic"}; + original.services = {"service", "other_service"}; original.rmw_serialization_format = "cdr"; original.topic_polling_interval = std::chrono::milliseconds{200}; original.regex = "[xyz]/topic"; - original.exclude = "*"; + original.exclude_topics = "*"; + original.exclude_services = "*"; original.node_prefix = "prefix"; original.compression_mode = "stream"; original.compression_format = "h264"; @@ -45,9 +48,11 @@ TEST(record_options, test_yaml_serialization) auto reconstructed = reconstructed_node.as(); #define CHECK(field) ASSERT_EQ(original.field, reconstructed.field) - CHECK(all); + CHECK(all_topics); + CHECK(all_services); CHECK(is_discovery_disabled); CHECK(topics); + CHECK(services); CHECK(rmw_serialization_format); #undef CMP } diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp index a20e48cdf4..7af1a6a815 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp @@ -24,12 +24,14 @@ #include "rosbag2_test_common/publication_manager.hpp" #include "rosbag2_test_common/wait_for.hpp" +#include "rosbag2_test_common/client_manager.hpp" #include "rosbag2_transport/recorder.hpp" #include "test_msgs/msg/arrays.hpp" #include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" +#include "test_msgs/srv/basic_types.hpp" #include "record_integration_fixture.hpp" @@ -58,7 +60,8 @@ TEST_F(RecordIntegrationTestFixture, regex_topics_recording) ASSERT_FALSE(std::regex_match(b3, re)); ASSERT_FALSE(std::regex_match(b4, re)); - rosbag2_transport::RecordOptions record_options = {false, false, {}, "rmw_format", 10ms}; + rosbag2_transport::RecordOptions record_options = + {false, false, false, {}, {}, "rmw_format", 10ms}; record_options.regex = regex; // TODO(karsten1987) Refactor this into publication manager @@ -99,7 +102,7 @@ TEST_F(RecordIntegrationTestFixture, regex_topics_recording) EXPECT_TRUE(recorded_topics.find(v1) != recorded_topics.end()); } -TEST_F(RecordIntegrationTestFixture, regex_and_exclude_recording) +TEST_F(RecordIntegrationTestFixture, regex_and_exclude_topic_recording) { auto test_string_messages = get_messages_strings(); auto test_array_messages = get_messages_arrays(); @@ -129,9 +132,10 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_recording) ASSERT_TRUE(std::regex_match(e1, re)); ASSERT_TRUE(std::regex_match(e1, exclude)); - rosbag2_transport::RecordOptions record_options = {false, false, {}, "rmw_format", 10ms}; + rosbag2_transport::RecordOptions record_options = + {false, false, false, {}, {}, "rmw_format", 10ms}; record_options.regex = regex; - record_options.exclude = topics_regex_to_exclude; + record_options.exclude_topics = topics_regex_to_exclude; // TODO(karsten1987) Refactor this into publication manager @@ -174,3 +178,69 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_recording) EXPECT_TRUE(recorded_topics.find(v1) != recorded_topics.end()); EXPECT_TRUE(recorded_topics.find(v2) != recorded_topics.end()); } + +TEST_F(RecordIntegrationTestFixture, regex_and_exclude_service_recording) +{ + std::string regex = "/[a-z]+_nice(_.*)"; + std::string services_regex_to_exclude = "/[a-z]+_nice_[a-z]+/(.*)"; + + // matching service + std::string v1 = "/awesome_nice_service"; + std::string v2 = "/still_nice_service"; + + // excluded topics + std::string e1 = "/quite_nice_namespace/but_it_is_excluded"; + + // service that shouldn't match + std::string b1 = "/numberslike1arenot_nice"; + std::string b2 = "/namespace_before/not_nice"; + + rosbag2_transport::RecordOptions record_options = + {false, false, false, {}, {}, "rmw_format", 10ms}; + record_options.regex = regex; + record_options.exclude_services = services_regex_to_exclude; + + auto service_manager_v1 = + std::make_shared>(v1); + + auto service_manager_v2 = + std::make_shared>(v2); + + auto service_manager_e1 = + std::make_shared>(e1); + + auto service_manager_b1 = + std::make_shared>(b1); + + auto service_manager_b2 = + std::make_shared>(b2); + + auto recorder = std::make_shared( + std::move(writer_), storage_options_, record_options); + recorder->record(); + + start_async_spin(recorder); + + ASSERT_TRUE(service_manager_v1->check_service_ready()); + ASSERT_TRUE(service_manager_v2->check_service_ready()); + ASSERT_TRUE(service_manager_e1->check_service_ready()); + ASSERT_TRUE(service_manager_b1->check_service_ready()); + ASSERT_TRUE(service_manager_b2->check_service_ready()); + + auto & writer = recorder->get_writer_handle(); + MockSequentialWriter & mock_writer = + static_cast(writer.get_implementation_handle()); + + ASSERT_TRUE(service_manager_v1->send_request()); + ASSERT_TRUE(service_manager_v2->send_request()); + ASSERT_TRUE(service_manager_e1->send_request()); + ASSERT_TRUE(service_manager_b1->send_request()); + ASSERT_TRUE(service_manager_b2->send_request()); + auto recorded_messages = mock_writer.get_messages(); + EXPECT_THAT(recorded_messages, SizeIs(4)); + + auto recorded_topics = mock_writer.get_topics(); + EXPECT_THAT(recorded_topics, SizeIs(2)); + EXPECT_TRUE(recorded_topics.find(v1 + "/_service_event") != recorded_topics.end()); + EXPECT_TRUE(recorded_topics.find(v2 + "/_service_event") != recorded_topics.end()); +} diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp index 16d31835e7..32b78f27a6 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp @@ -68,7 +68,7 @@ class RecordSrvsTest : public RecordIntegrationTestFixture client_node_ = std::make_shared("test_record_client"); rosbag2_transport::RecordOptions record_options = - {false, false, {test_topic_}, "rmw_format", 100ms}; + {false, false, false, {test_topic_}, {}, "rmw_format", 100ms}; storage_options_.snapshot_mode = snapshot_mode_; storage_options_.max_cache_size = 200; recorder_ = std::make_shared( diff --git a/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp b/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp index 3271d12c47..b8ae64d6bd 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp @@ -93,7 +93,7 @@ TEST_P(TestRewrite, test_noop_rewrite) { output_storage.uri = (output_dir_ / "unchanged").string(); output_storage.storage_id = storage_id_; rosbag2_transport::RecordOptions output_record; - output_record.all = true; + output_record.all_topics = true; output_bags_.push_back({output_storage, output_record}); rosbag2_transport::bag_rewrite(input_bags_, output_bags_); @@ -114,7 +114,7 @@ TEST_P(TestRewrite, test_merge) { output_storage.uri = (output_dir_ / "merged").string(); output_storage.storage_id = storage_id_; rosbag2_transport::RecordOptions output_record; - output_record.all = true; + output_record.all_topics = true; output_bags_.push_back({output_storage, output_record}); rosbag2_transport::bag_rewrite(input_bags_, output_bags_); @@ -146,7 +146,7 @@ TEST_P(TestRewrite, test_message_definitions_stored_with_merge) { output_storage.uri = (output_dir_ / "merged").string(); output_storage.storage_id = storage_id_; rosbag2_transport::RecordOptions output_record; - output_record.all = true; + output_record.all_topics = true; output_bags_.push_back({output_storage, output_record}); rosbag2_transport::bag_rewrite(input_bags_, output_bags_); @@ -190,8 +190,8 @@ TEST_P(TestRewrite, test_filter_split) { storage_opts.uri = (output_dir_ / "split1").string(); storage_opts.storage_id = storage_id_; rosbag2_transport::RecordOptions rec_opts; - rec_opts.all = true; - rec_opts.exclude = "basic"; + rec_opts.all_topics = true; + rec_opts.exclude_topics = "basic"; output_bags_.push_back({storage_opts, rec_opts}); } { @@ -199,7 +199,7 @@ TEST_P(TestRewrite, test_filter_split) { storage_opts.uri = (output_dir_ / "split2").string(); storage_opts.storage_id = storage_id_; rosbag2_transport::RecordOptions rec_opts; - rec_opts.all = false; + rec_opts.all_topics = false; rec_opts.topics = {"b_basictypes"}; output_bags_.push_back({storage_opts, rec_opts}); } @@ -234,7 +234,7 @@ TEST_P(TestRewrite, test_compress) { output_storage.uri = out_bag.string(); output_storage.storage_id = storage_id_; rosbag2_transport::RecordOptions output_record; - output_record.all = true; + output_record.all_topics = true; output_record.compression_mode = "file"; output_record.compression_format = "zstd"; output_bags_.push_back({output_storage, output_record}); diff --git a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp index a4ce8c2fea..c22d2047f1 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp @@ -35,7 +35,10 @@ class RegexFixture : public Test {"/invalidated_topic", {"invalidated_topic_type"}}, {"/localization", {"localization_topic_type"}}, {"/invisible", {"invisible_topic_type"}}, - {"/status", {"status_topic_type"}} + {"/status", {"status_topic_type"}}, + {"/invalid_service/_service_event", {"service/srv/invalid_service_Event"}}, + {"/invalidated_service/_service_event", {"service/srv/invalidated_service_Event"}}, + {"/planning_service/_service_event", {"service/srv/planning_service_Event"}} }; }; @@ -58,6 +61,7 @@ TEST(TestTopicFilter, filter_hidden_topics) { } { rosbag2_transport::RecordOptions record_options; + record_options.all_topics = true; record_options.include_hidden_topics = false; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types); @@ -72,8 +76,9 @@ TEST(TestTopicFilter, filter_topics_with_more_than_one_type) { {"topic/c", {"type_c", "type_c2"}}, {"topic/d", {"type_d", "type_d", "type_d2"}}, }; - - rosbag2_transport::TopicFilter filter{rosbag2_transport::RecordOptions{}, nullptr, true}; + rosbag2_transport::RecordOptions record_options; + record_options.all_topics = true; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types); EXPECT_THAT(filtered_topics, SizeIs(2)); for (const auto & topic : @@ -89,8 +94,9 @@ TEST(TestTopicFilter, filter_topics_with_known_type_invalid) { {"topic/b", {"type_b"}}, {"topic/c", {"type_c"}} }; - - rosbag2_transport::TopicFilter filter{rosbag2_transport::RecordOptions{}, nullptr}; + rosbag2_transport::RecordOptions record_options; + record_options.all_topics = true; + rosbag2_transport::TopicFilter filter{record_options, nullptr}; auto filtered_topics = filter.filter_topics(topics_and_types); ASSERT_EQ(0u, filtered_topics.size()); } @@ -101,7 +107,9 @@ TEST(TestTopicFilter, filter_topics_with_known_type_valid) { {"topic/b", {"test_msgs/BasicTypes"}}, {"topic/c", {"test_msgs/BasicTypes"}} }; - rosbag2_transport::TopicFilter filter{rosbag2_transport::RecordOptions{}, nullptr}; + rosbag2_transport::RecordOptions record_options; + record_options.all_topics = true; + rosbag2_transport::TopicFilter filter{record_options, nullptr}; auto filtered_topics = filter.filter_topics(topics_and_types); ASSERT_EQ(3u, filtered_topics.size()); } @@ -110,7 +118,10 @@ TEST(TestTopicFilter, filter_topics) { std::map> topics_and_types { {"topic/a", {"type_a"}}, {"topic/b", {"type_b"}}, - {"topic/c", {"type_c"}} + {"topic/c", {"type_c"}}, + {"/service/a/_service_event", {"service/srv/type_a_Event"}}, + {"/service/b/_service_event", {"service/srv/type_b_Event"}}, + {"/service/c/_service_event", {"service/srv/type_c_Event"}}, }; { @@ -151,13 +162,38 @@ TEST(TestTopicFilter, filter_topics) { EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()); } } + + { + rosbag2_transport::RecordOptions record_options; + record_options.services = {"/service/a/_service_event"}; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types); + ASSERT_EQ(1u, filtered_topics.size()); + EXPECT_EQ("/service/a/_service_event", filtered_topics.begin()->first); + } + + { + rosbag2_transport::RecordOptions record_options; + record_options.services = { + "/service/a/_service_event", + "/service/b/_service_event", + "/service/d/_service_event"}; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types); + ASSERT_EQ(2u, filtered_topics.size()); + for (const auto & topic : + {"/service/a/_service_event", "/service/b/_service_event"}) + { + EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()); + } + } } -TEST_F(RegexFixture, regex_all_and_exclude) +TEST_F(RegexFixture, regex_all_topics_and_exclude_topics) { rosbag2_transport::RecordOptions record_options; - record_options.exclude = "/inv.*"; - record_options.all = true; + record_options.exclude_topics = "/inv.*"; + record_options.all_topics = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_); @@ -167,38 +203,100 @@ TEST_F(RegexFixture, regex_all_and_exclude) } } -TEST_F(RegexFixture, regex_filter_exclude) +TEST_F(RegexFixture, regex_all_services_and_exclude_services) { rosbag2_transport::RecordOptions record_options; - record_options.regex = "/invalid.*"; - record_options.exclude = ".invalidated.*"; - record_options.all = false; + record_options.exclude_services = "/inv.*"; + record_options.all_services = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_); EXPECT_THAT(filtered_topics, SizeIs(1)); + EXPECT_EQ("/planning_service/_service_event", filtered_topics.begin()->first); +} + +TEST_F(RegexFixture, regex_all_topics_all_services_and_exclude_topics_and_services) +{ + rosbag2_transport::RecordOptions record_options; + record_options.exclude_topics = "/inv.*"; + record_options.all_topics = true; + record_options.exclude_services = "/inv.*"; + record_options.all_services = true; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types_); + + EXPECT_THAT(filtered_topics, SizeIs(4)); + for (const auto & topic : + {"/planning", "/localization", "/status", "/planning_service/_service_event"}) + { + EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()); + } +} + +TEST_F(RegexFixture, regex_filter_exclude_topics) +{ + rosbag2_transport::RecordOptions record_options; + record_options.regex = "/invalid.*"; + record_options.exclude_topics = ".invalidated.*"; // Only affect topics + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types_); + + EXPECT_THAT(filtered_topics, SizeIs(3)); EXPECT_TRUE(filtered_topics.find("/invalid_topic") != filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find("/invalid_service/_service_event") != filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find("/invalidated_service/_service_event") != filtered_topics.end()); +} + +TEST_F(RegexFixture, regex_filter_exclude_services) +{ + rosbag2_transport::RecordOptions record_options; + record_options.regex = "/invalid.*"; + record_options.exclude_services = ".invalidated.*"; // Only affect services + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types_); + + EXPECT_THAT(filtered_topics, SizeIs(3)); + EXPECT_TRUE(filtered_topics.find("/invalid_topic") != filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find("/invalidated_topic") != filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find("/invalid_service/_service_event") != filtered_topics.end()); +} + +TEST_F(RegexFixture, regex_filter_exclude_topics_and_services) +{ + rosbag2_transport::RecordOptions record_options; + record_options.regex = "/invalid.*"; + record_options.exclude_topics = ".invalidated.*"; + record_options.exclude_services = ".invalidated.*"; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types_); + + EXPECT_THAT(filtered_topics, SizeIs(2)); + EXPECT_TRUE(filtered_topics.find("/invalid_topic") != filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find("/invalid_service/_service_event") != filtered_topics.end()); } TEST_F(RegexFixture, regex_filter) { rosbag2_transport::RecordOptions record_options; record_options.regex = "^/inval"; - record_options.all = false; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_); - EXPECT_THAT(filtered_topics, SizeIs(2)); - for (const auto & topic : {"/invalid_topic", "/invalidated_topic"}) { + EXPECT_THAT(filtered_topics, SizeIs(4)); + for (const auto & topic : + {"/invalid_topic", "/invalidated_topic", "/invalid_service/_service_event", + "/invalidated_service/_service_event"}) + { EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()); } } -TEST_F(RegexFixture, regex_all_and_filter) +TEST_F(RegexFixture, regex_all_topics_and_filter) { rosbag2_transport::RecordOptions record_options; record_options.regex = "/status"; - record_options.all = true; + record_options.all_topics = true; + record_options.exclude_services = ".*"; // Not include services rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_); EXPECT_THAT(filtered_topics, SizeIs(6)); @@ -209,7 +307,7 @@ TEST_F(RegexFixture, do_not_print_warning_about_unknown_types_if_topic_is_not_se rosbag2_transport::RecordOptions record_options; // Select only one topic with name "/planning" via topic list record_options.topics = {"/planning"}; - record_options.all = false; + record_options.all_topics = false; rosbag2_transport::TopicFilter filter{record_options, nullptr, false}; testing::internal::CaptureStderr(); auto filtered_topics = filter.filter_topics(topics_and_types_); @@ -227,7 +325,7 @@ TEST_F(RegexFixture, do_not_print_warning_about_unknown_types_if_topic_is_not_se rosbag2_transport::RecordOptions record_options; // Select only one topic with name "/planning" via regex record_options.regex = "^/planning"; - record_options.all = false; + record_options.all_topics = false; rosbag2_transport::TopicFilter filter{record_options, nullptr, false}; testing::internal::CaptureStderr(); auto filtered_topics = filter.filter_topics(topics_and_types_); @@ -241,3 +339,25 @@ TEST_F(RegexFixture, do_not_print_warning_about_unknown_types_if_topic_is_not_se "Topic '/planning' has unknown type 'planning_topic_type'") != std::string::npos); } } + +TEST_F(RegexFixture, regex_all_services_and_filter) +{ + rosbag2_transport::RecordOptions record_options; + record_options.regex = "/status"; + record_options.all_services = true; + record_options.exclude_topics = ".*"; // Not include topics + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types_); + EXPECT_THAT(filtered_topics, SizeIs(3)); +} + +TEST_F(RegexFixture, regex_all_topics_all_services_and_filter) +{ + rosbag2_transport::RecordOptions record_options; + record_options.regex = "/status"; + record_options.all_topics = true; + record_options.all_services = true; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types_); + EXPECT_THAT(filtered_topics, SizeIs(9)); +} diff --git a/rosbag2_transport/test/srv/SimpleTest.srv b/rosbag2_transport/test/srv/SimpleTest.srv new file mode 100644 index 0000000000..afdafd5367 --- /dev/null +++ b/rosbag2_transport/test/srv/SimpleTest.srv @@ -0,0 +1,3 @@ +int64 input +--- +int64 output \ No newline at end of file