From d280d3f873bb62b6aaff62855c1ce5cc6b0f37b5 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 11 Oct 2023 15:03:01 +0800 Subject: [PATCH 01/35] 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 9800a63cf8..3ac49a2366 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 @@ -259,6 +260,12 @@ if(BUILD_TESTING) if(TARGET test_time_controller_clock) target_link_libraries(test_time_controller_clock ${PROJECT_NAME}) endif() + + ament_add_gmock(test_service_utils + test/rosbag2_cpp/test_service_utils.cpp) + if(TARGET test_service_utils) + target_link_libraries(test_service_utils ${PROJECT_NAME}) + endif() endif() ament_package() 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 1f90a8a171..06b2316abf 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -48,6 +48,14 @@ target_link_libraries(_compression_options PUBLIC rosbag2_compression::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 3593fc9fd8..cf2831f9bb 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -350,13 +350,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) @@ -371,6 +372,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 50621877f8..ee181184df 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 ec6a0a9813..701eb64fbb 100644 --- a/rosbag2_transport/src/rosbag2_transport/record_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/record_options.cpp @@ -27,13 +27,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; @@ -50,9 +53,11 @@ Node convert::encode( bool convert::decode( const Node & node, rosbag2_transport::RecordOptions & record_options, int version) { - 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); @@ -60,7 +65,8 @@ bool convert::decode( 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 86b4f7e43e..72d8cf58c1 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" @@ -476,6 +477,13 @@ void RecorderImpl::subscribe_topic(const rosbag2_storage::TopicMetadata & topic) writer_->create_topic(topic); rosbag2_storage::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 b2c456affe..275b4c1048 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp @@ -184,7 +184,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 48e0d95516..4915a975d1 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(); @@ -213,7 +214,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(); @@ -256,7 +257,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(); @@ -300,7 +301,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(); @@ -346,7 +348,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(); @@ -384,7 +387,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 d95d21c644..847f647afe 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 CHECK } 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 ee5ec074b4..4a951107dd 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_); @@ -144,7 +144,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_); @@ -188,8 +188,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}); } { @@ -197,7 +197,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}); } @@ -232,7 +232,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 From cb0447cd75c017f135c7a0001ac7c43daaf9ab2c Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 13 Oct 2023 17:03:14 +0800 Subject: [PATCH 02/35] Address MichaelOrlov's review comments Signed-off-by: Barry Xu --- ros2bag/ros2bag/verb/record.py | 6 ++-- rosbag2_cpp/src/rosbag2_cpp/info.cpp | 29 +++++++++++-------- rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 22 +++++++------- .../src/rosbag2_py/format_bag_metadata.cpp | 13 ++++----- .../src/rosbag2_transport/topic_filter.cpp | 23 ++++++--------- 5 files changed, 47 insertions(+), 46 deletions(-) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 3b3e0ed1da..e7a1ed3ce9 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -185,8 +185,8 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '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 + # At least one options out of --all, --all-topics, --all-services, --services, topics or + # --regex must be used if not (args.all or args.all_topics or args.all_services or args.services or (args.topics and len(args.topics) > 0) or args.regex): return False @@ -195,7 +195,7 @@ def _check_necessary_argument(self, args): def main(self, *, args): # noqa: D102 if not self._check_necessary_argument(args): - return print_error('Must specify only one option out of --all, --all-topics, ' + return print_error('Need to specify one option out of --all, --all-topics, ' '--all-services, --services, topics and --regex') # Only one option out of --all, --all-services --services or --regex can be used diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index cf4faade5c..9e00cb742c 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -19,18 +19,15 @@ #include #include -#include "rcl/service_introspection.h" #include "rmw/rmw.h" - +#include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rcpputils/filesystem_helper.hpp" +#include "service_msgs/msg/service_event_info.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 { @@ -63,6 +60,9 @@ namespace { struct client_id_hash { + static_assert( + std::is_same, + service_msgs::msg::ServiceEventInfo::_client_gid_type>::value); std::size_t operator()(const std::array & client_id) const { std::hash hasher; @@ -75,7 +75,7 @@ struct client_id_hash } }; -using client_id = std::array; +using client_id = service_msgs::msg::ServiceEventInfo::_client_gid_type; using sequence_set = std::unordered_set; struct service_req_resp_info { @@ -93,6 +93,11 @@ std::vector> Info::read_service_info( throw std::runtime_error("No plugin detected that could open file " + uri); } + rosbag2_storage::ReadOrder read_order; + if (!storage->set_read_order(read_order)) { + throw std::runtime_error("Failed to set read order on " + uri); + } + using service_analysis = std::unordered_map>; @@ -134,19 +139,19 @@ std::vector> Info::read_service_info( reinterpret_cast(&msg)); if (ret != RMW_RET_OK) { throw std::runtime_error( - "It failed to deserialize message from " + bag_msg->topic_name + " !"); + "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); + 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); + service_process_info[bag_msg->topic_name]->response[msg.client_gid].emplace( + msg.sequence_number); break; } } diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp index 19cecf3fd5..a456cb62ce 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -17,6 +17,7 @@ #include "rcl/service_introspection.h" #include "rosbag2_cpp/service_utils.hpp" +#include "rosidl_typesupport_cpp/message_type_support.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" @@ -59,8 +60,9 @@ bool is_service_event_topic(const std::string & topic, const std::string & topic std::string service_event_topic_name_to_service_name(const std::string & topic_name) { + std::string service_name; if (topic_name.length() <= (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)) { - return std::string(); + return service_name; } if (topic_name.substr( @@ -68,10 +70,10 @@ std::string service_event_topic_name_to_service_name(const std::string & topic_n (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)) != RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) { - return std::string(); + return service_name; } - std::string service_name = topic_name.substr( + service_name = topic_name.substr( 0, topic_name.length() - (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)); return service_name; @@ -79,22 +81,23 @@ std::string service_event_topic_name_to_service_name(const std::string & topic_n std::string service_event_topic_type_to_service_type(const std::string & topic_type) { + std::string service_type; if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { - return std::string(); + return service_type; } // Should include '/srv/' in type if (topic_type.find(service_event_topic_type_middle) == std::string::npos) { - return std::string(); + return service_type; } if (topic_type.substr(topic_type.length() - std::strlen(service_event_topic_type_postfix)) != service_event_topic_type_postfix) { - return std::string(); + return service_type; } - std::string service_type = topic_type.substr( + service_type = topic_type.substr( 0, topic_type.length() - strlen(service_event_topic_type_postfix)); return service_type; @@ -110,8 +113,7 @@ size_t get_serialization_size_for_service_metadata_event() } const rosidl_message_type_support_t * type_support_info = - rosidl_typesupport_cpp:: - get_message_type_support_handle(); + 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 = @@ -119,7 +121,7 @@ size_t get_serialization_size_for_service_metadata_event() type_support_info, rosidl_typesupport_introspection_cpp::typesupport_identifier); if (type_support_handle == nullptr) { - std::runtime_error("Cannot get ServiceEventInfo typesupport handle !"); + throw std::runtime_error("Cannot get ServiceEventInfo typesupport handle !"); } auto service_event_info = diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp index a2189bb1fa..c7ed84645d 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp @@ -138,6 +138,7 @@ void format_topics_with_type( } if (i == number_of_topics) { + info_stream << std::endl; return; } @@ -163,7 +164,7 @@ struct ServiceMetadata struct ServiceInformation { ServiceMetadata service_metadata; - size_t event_message_count; + size_t event_message_count = 0; }; std::vector> filter_service_event_topic( @@ -237,11 +238,9 @@ std::string format_bag_meta_data( 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); - } + service_info_list = filter_service_event_topic( + metadata.topics_with_message_count, + total_service_event_msg_count); info_stream << std::endl; info_stream << "Files: "; @@ -264,7 +263,7 @@ std::string format_bag_meta_data( if (!only_topic) { info_stream << "Service: " << service_info_list.size() << std::endl; info_stream << "Service information: "; - if (service_info_list.size() != 0) { + if (!service_info_list.empty()) { format_service_with_type(service_info_list, info_stream, indentation_spaces + 2); } } diff --git a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp index aee426d447..374156895f 100644 --- a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp +++ b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp @@ -176,6 +176,13 @@ bool TopicFilter::take_topic( { return false; } + + if (!record_options_.include_hidden_topics && topic_is_hidden(topic_name)) { + RCUTILS_LOG_WARN_ONCE_NAMED( + ROSBAG2_TRANSPORT_PACKAGE_NAME, + "Hidden topics are not recorded. Enable them with --include-hidden-topics"); + return false; + } } else { if (!record_options_.all_services && record_options_.services.empty() && @@ -191,8 +198,7 @@ bool TopicFilter::take_topic( } // Convert service event topic name to service name - auto service_name = - rosbag2_cpp::service_event_topic_name_to_service_name(topic_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() && @@ -202,8 +208,7 @@ bool TopicFilter::take_topic( } std::regex include_regex(record_options_.regex); - if ( - !record_options_.all_services && // All takes precedence over 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)) { @@ -211,16 +216,6 @@ bool TopicFilter::take_topic( } } - 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; - } - if (!allow_unknown_types_ && !type_is_known(topic_name, topic_type)) { return false; } From c0c099da012ace114bf772f1e94fda2b909af4ba Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 17 Oct 2023 15:15:35 +0800 Subject: [PATCH 03/35] Add tests for rosbag2_cpp::Info::read_service_info() Signed-off-by: Barry Xu --- rosbag2_cpp/CMakeLists.txt | 2 + .../test/rosbag2_cpp/bags/only_services.mcap | Bin 0 -> 4588 bytes .../test/rosbag2_cpp/bags/only_topics.mcap | Bin 0 -> 14654 bytes .../rosbag2_cpp/bags/topics_and_services.db3 | Bin 0 -> 45056 bytes .../rosbag2_cpp/bags/topics_and_services.mcap | Bin 0 -> 19039 bytes rosbag2_cpp/test/rosbag2_cpp/test_info.cpp | 83 ++++++++++++++++++ 6 files changed, 85 insertions(+) create mode 100644 rosbag2_cpp/test/rosbag2_cpp/bags/only_services.mcap create mode 100644 rosbag2_cpp/test/rosbag2_cpp/bags/only_topics.mcap create mode 100644 rosbag2_cpp/test/rosbag2_cpp/bags/topics_and_services.db3 create mode 100644 rosbag2_cpp/test/rosbag2_cpp/bags/topics_and_services.mcap diff --git a/rosbag2_cpp/CMakeLists.txt b/rosbag2_cpp/CMakeLists.txt index 3ac49a2366..953495e1ea 100644 --- a/rosbag2_cpp/CMakeLists.txt +++ b/rosbag2_cpp/CMakeLists.txt @@ -138,6 +138,8 @@ if(BUILD_TESTING) find_package(rosbag2_test_msgdefs REQUIRED) ament_lint_auto_find_test_dependencies() + add_definitions(-D_TEST_RESOURCES_DIR_PATH="${CMAKE_CURRENT_SOURCE_DIR}/test/${PROJECT_NAME}") + add_library( converter_test_plugins SHARED diff --git a/rosbag2_cpp/test/rosbag2_cpp/bags/only_services.mcap b/rosbag2_cpp/test/rosbag2_cpp/bags/only_services.mcap new file mode 100644 index 0000000000000000000000000000000000000000..4e8d72a0feee59a2997e2257a6ac969930ae2b68 GIT binary patch literal 4588 zcmeHLPiP!f7~gG@CT_A#rIu(baT*0_%O*3kJNst>LeOebTa`uywK}|=d7B;Bof&5) zn?_OT&0YjS#0Xw;(~E)!@ox))9$OSWcoM3ph~Pyp67c(G-j~kKuG<7Xc<8%)@B8!K z_r5=G-f#E9>3bI*k@LCiZJBrs1M%HJ%>!-O^|r}8Ntsn<<%u7%vBhX?QD-aaHy}Cy zY`}e+we07(Wwd$7EEcklK-J-XV7rbkRdYe;`fP<8w#A*$ZrI${CEPn_t@B@AKK1}5!!8@(i?&8nB3r=Yc_SMrmDqaMU{)?s#dC0w5nXmwRsSr zN7G$(LfkHNJ=+Y7H9Ks=DU(WY{nXPr(`|b`$l)+X!}Z$?R?W z8g`2t9t)cR9x@pL>QDmqRQZ&$$ItfNkcqmMy;QArrf@JJAo!<|>q< zZD5&n>BStB+pz2)^j%%@-Bt@yzW1lSaj`!={@|h7Bqk(Sb$~t)#Ytm1$T7*5#7Kaxtyv z3Wb802dnMe@Ryb}*k~mSu!%E_w;(3~0iRjXi-o4}spa{5N39EiiQ(4empC zJ`16Gz8jTh8gvDz34y!*x-JzVR~GlMP?$ETu+8dr%ML-Opbi?fi|5ROR7H&m#pGtP z4Wqm}k?>kaN>a;i@W5kHv+sOO2ae)EW5(xbLEFW=(c&!NMt2A7>>#Y;+LnQZcNnyX z2Ni;g;$Y^W{lQa%wE{i~nTu|X5w3gOXtJRB0Eo^kEiTK7QB+OEQcAL_)y#^*HM6K` zilWs@YTc|C6{}IJ)=R~r%u7~HF4vWMMKjI1V%F+Klb6lv$TP4Pj<~GUX+%+snj=SM z`DfqVe>^++>kF-8Z+{gn$sEA%lvvqIvHCPd&WKs;X6la9Q9~2OJ@$YFx~Xg}4RnJW z;VzMHps5HpK0H{(J3EB{`}HvzF!3B8VB0pAbvmu+09!cH$-tQE<-m9udNSs zgD%k32u@(6BACDD>SGuIQM5&K+*&< zxE;Ep?@%g&_R;qOjDRRba3@6Y(92=pUR0O{m zr=qulC`E7=Bbi(-CW)O{6IH&YIJoIPGF3F5mOLef*E~- zisCxb$Kp-F(+&Sa@N^Z=isRD#`9Sd0=+mOLs5{pMPi^$Q;Hd}ZMaHQ}n}VmzZV8@J zV2QL%oD>_02Nto~Jc#}Qt4W~xS6EHeWLc3*vZCz*v?=9UMN^fU2FR^ks%T2B2ec{b zVANKS;^&D#Ct@%h_^9d9AbcPU@mR5g(<*+!fnK{+hqhY0CdFrkRXjZcDM1)fX7UtJch*s)3-eY zCJ_YDrT++kL{3uR7&`9%{X3xFt-*%qMuA;FBy=fpKhd^N%-8)zFe$z$(RcC&+P{JJ zcZv4LtKx;75EOsK#RDHvoYSjIuURgVll|Dq*t*ATN<6~9i^nfsHY%R>%VHH0`msth ze^@Aw3HqBjQsUX%w)s|UxZL}+?z>s6A1d#QXQmKrYlb(3=8AB!tr@rX$HoikHDm7x h&2n0k-x8Wdq1l5!a&gcBPKYDDKKjHb7sP)&e*^gyIb;9; literal 0 HcmV?d00001 diff --git a/rosbag2_cpp/test/rosbag2_cpp/bags/only_topics.mcap b/rosbag2_cpp/test/rosbag2_cpp/bags/only_topics.mcap new file mode 100644 index 0000000000000000000000000000000000000000..3a4abb7c639dd7d633d30d9129bf61860c1abcfd GIT binary patch literal 14654 zcmeHOO^hW~6@D{4!{CDf{{kTs;X0j&kk{}3x07DiQJ2bwL^m21#z-b&zDc#S5nUzypkwwp2s`z?XkmS)~GhB^*jG`N{u+D zM%aAV{2iJ;gU?hXJ`en_3fCHojJrJJZ=-8lB&i>T%dB5cvnb(1Vfn5IGvD(?vdjo} z*_|Xjqd{fHLLNq`aH7ynmsuTic#`?y(8~O=nCZ7tKXim8;>Z~-vq7`jYW14+R=3}2 z_j;Xvy;mNKG^G`tXcA@wE{kH{Nv$0}8v)B=CzgKZxv~?D;{@$M#`2N~J}% zHS$w~DT4@P+z(U6S!|{;LG!BRMRrLze3A;5jl^s>+u@iLGLHTE4x7Z-qo@E(8Vd(v z=M*M{KnKtq43ENKm)U~(^i#0Nld!W(N{1Cet zi3A2}k&Ut}PM2#nSET+htdbqAM#->7cFch_s5wF8ykPOrl{T$9%Eo~aEp7N9`{Niw z7-ki=4Y6=p1T)Hb5I{P!R+Abh0urgdFv|can2!BGAlalY2{DyDQ?Iaw7(7%hPZShA zwOqc*BwkT8p0NhG{1_sG&V-z{QzZK!f+2U&22|OVjA1mXY)tZ=B@-uOWYVckgp)C- z1h51SOmGrE#=%1nQwSan9r?~k(NF?%$cpFc?Li%3f}!Ku1mxPXr-)=VB;p0*^I)n$ zD)3}dU`@0sNuf3g4_LMvi?jkn$m%?w6X7Z1XBBqvd2GQgbZlX~Y_FE52dlOI-OQ4B z0`n(R2s{Ytk}V%Y{54HA#X|8?#vZn$lrq?~zy-S!X-3I*=R1+RyS}=y`o!AS+J<#) zeaqTh+XDAotz^=)5g&6NNaG$x8JRhN`l9c#VU%See29U^5DO@1=x5p!q17bLYJHLo zuz_q3bYYMD1uA307SYBTD8$QoJukQ6M0f5?Do^dJs_o&4G1+8ez)`^LUU#8*Gea^q*oQA%6vj`4A%& zE@VdNRPd5$%$5cfxL+0cUlllD6?kA3xL}oj9b1nu^`P#av|-&gP?fOv7F%U*G_gS< z_+ySw4jJ=;2o4vPj3W%!Xn1f|q8)0zvbMIydYyK)EC&Lpf>4B+K{9dB>bcTpgb|kD zumKL*0}&2kM*@HB!_Cm=u6h-~z_&;ezDr!QcX1e07+euop1NTaE}2E9$WH@tR?^Fo zvk!n*WDzVpae$@~BMzC?Z7U`+7G2Kx2XbAIY3_zNq0HtYq2qaB4DJ7Vy zJ~301u{iV0`=^b^c)Ii{{H*1=W=TgQT|lGbCU0TfXYn)gJRGyG^&;F=D4cY!SJ1b_ z;DkY!c9&TT{;n%xa<<9!g_rJg+YkHqwMIN0J%*-@E01s18&6 z$99^$-s-iw4cF$qMhl%?-n9o_+wOM_cOZdd_}dk5Fh0?tD$& z?abStb>Z^gk3{PMK#~_(ML*+n5&8%UZ2OKVotlTzytY}i&%Qf_=GVUYbBU&T8#G_N z?cUQzQLh6?WH-o{P7s4gtNW}?HO7(Sr#KF)duF0U;gC5GuB@f)h++VNnA1JgboNdC zsi<9f`xkN>=54m|$gB4qxs4_u9e*3zEY8^j-PKEokaICSPv zo_0aPqedM=cz#4P@F?o%VHh2bG25v8WcA2xkg6(^LdS&X4<`xYaYc(WA74Tj;UNx( z%q2_?d(Lg^-qWQRzl@=~hpZPKV_{y`-$s?U?o$0mCoYg##$$x+_xjs#@7teL{e~Q0 zCjSA?oxdhz@f2c|@1W^`808Ppbwe@AZl~27G&>(6QrYMZdYxvY+pO1Hjb5`|AM|G< zmA%6wl}qrd5y;E~trT~phfcX=R#UE1Z71>JY^E`MAKg#q8^!lh3fvYWQWTtfnuc%E z=r!^m^b`};l>yp3CJL8K_s{dl5FRah9M6V(9vS^EXa`&&*<19<)Nt*T(Z@ayOEu28 zWz?AaWgm8$am6$~2m59p4)F2Zvx^1pSjE$mJdtIl{g96EI(@I#sCRkC@w~p*;k`k- z-t0S#exuP7qS>f7I{i-FbseYE?YUmN4R53YCuGpAdwsdczA!Mws}Gf>S%PjP&MVB9Y7WiXqk_C*fXm;8T?<402!)$v4+!R}OuidRT9oP06jjnK8ZO`Uj zbAZ>t>o;6J@LG+&;LbexKxXb^or9d`ywDLn9le-JVlFe|BhkQ_BdB+bOyu3g8`#2k z$g#t7szf)wIY$sQqmWvp-;0ao#YMJBL3qT05aq*pPYFicTEi@*KKi#c638e9(NM-U z<)9QnJ&G^NNbB|8Y=ki6CMLN&acN4P?1`MT8q0o}>}l1IH&1x7ThriVnrBgeEGgIo zU=`th3XS{YI1ppFT`D%6I1^ke5w%X)jtGL{QbUAmCw3K5QiGvr*7VSt9Yl7m)q$MB z2dH=3qVBX>yj}0M9NQVRd&0Ios4Q<#zyeVQCz!Y*pPi$*c(s9G;#K&iiX@1CBRsW@ zjF*fq$slJW1I9!=qQfl35D}MYR2=$xHx#BG`>u;a4cC{)q|amQ_h5404G5&1{-StbHdE`Y=m5cH9X04tLaA>|oRv=a)ZKPv3WkP;Sz zS}a@$A~Ms3kg-WNP);Ki$V#bfvrF@&fcj}FlYo@J>$6AKE8yyp5jPf;YffR-xC1$~=S2wP$wB&+oNrGvbX#19v z`Dw~{Q)Yv5L`c#^0j2g7iPknY);Ct#Ifs8|@%< zqNZP7-CDi6(xK_noLrWPEzM3!1%uR6H38bv$&}SlggWzNtAviq%pZ_>Ca#TRV&x-w zs)_J|j4QE|DMUgXS%0Z1NO{4W8C0dx(DW@xfXrorwV(!Z0$@lkNL?uFL&XaQQHQdg zl+Q6ydpXy@k%>{lKo8Fc;qsV|K`cyyKNuv%?DOH1P=+~=nxco9X%sfkEy*D(>dZC` zzg3yi0~jbtU!?3_F8hDDbl{`_2sU5UD4i4l?WOtYAet&sIw=4;DFC`|YS4_Sgteq| z$s8Q;B2Nl{M6z{K03^jC4?~_50IA9&_;Ju2(in;m>6)XH0-%!upuG~U|AGRbg+Izr zPN}p|`T_-Wj+L1z1a~Mis%|;*b|TBBbrB-nGnC^&_2395P+lLw7F>Bzfs1-AQ{JV% zWEH=@G3|qeqG>=V!gCTK>j>pBMYk#x&!JE)O6dM*AX9xRik0Mz=gbdv#`+WjSF{6) z-r(M;3!J`%-h&F9?m}NNxR0>maSNPe!IKHp%5dZH>WcD2+TI+57ASnDW3>1^6{4N` zzU2i?hga;}Xjt~7DvhetZaEWAs?rWE+B;mvZ!|S^=3Q-$7GQd*G-6KCfHqCvJH#~I z|Itm;|4HrT;cUFor2ZX!u#O%&QRt!5MLkq3M88e_jj5BNy06pu_}jmvM(-PTuPQbw z_4OZjscGwUM&bE}dddIIpMRP6`3t)9Vg3F%W@(7ljCI;{X5v literal 0 HcmV?d00001 diff --git a/rosbag2_cpp/test/rosbag2_cpp/bags/topics_and_services.db3 b/rosbag2_cpp/test/rosbag2_cpp/bags/topics_and_services.db3 new file mode 100644 index 0000000000000000000000000000000000000000..22714632fa584f815fb645d69831509465adb032 GIT binary patch literal 45056 zcmeHQTZ|;hRjulGSI=xA26?b%mkp+6JhN2S``LI5J-f9_dwZvc?q1K1W=w8AqN);S zW!7Y7b#EiE)w4!65?YXKe;{m2fQ0|}03>515Pu*U1jvZj2jYuG+7BcPkOc^FZsen~ zx~m`Ko_#t}JyV&H5jSp}dm}RMjmWs$KeJ)_oOK+xullT#`hiqBo%$)pQmNF___u_A z(J_aUndkz)>BRYQr%$KycRq0ucjqqrY%2TZZ1#!f#g{I80~hj;2uK7Z0uljBL^ zmW6HJVo^4BExyIpwx7LzaA|h_@>5Tx5B7akvv`y(`kf1@?yTM1XvajZnFi|FX}{3k zVz;-}Us~I`%U*2X&H2v2)WKAI^Va4LyY=$MMs8GTR4%^74AocJ&5g~QEK0k6(3_dR zT&biV?j^ZW4Bjzq(>ER43mHr%%}(Mqnc-x%{rv%FJMCA%al|#Zb;mI6?xbXHC`WlX zl#xh+0Z(_$0dcMLRIfLl=la3U^!()ye>na6v}me;&~uQZMdKbgo-%Nqj%jgkoaNk^nl(KY=+)g&g1$;}?(^VxdaP+t z>T!*W?kl_8^?>71{fcsob9&uO3v3To1QVX#<9#*we`<_@AnjW4bEKA}io1ZtFg@RO z4pl|Ac{=+aDg4PpA|Mfv2uK7Z0uljRrfL_i`S5s(N-1m0x?9)4zaF?FSN>2uc)t`x9_%^CWxZYieibGM`F z+$;3GZehde>V~_*dRXA*xcjTDl3~o?1HZS*ia2$-WvZHKnLh57=-$vBzvij|XETtLkb~)%0Rr)jF+mqfyZs)q1h48(ODSs&k`K?PzMJ+`><*(<~Wkt5Yd8xvIZj zomot^?_K)bIs~8^hT`uz1ix2MuqboaMBSWf=C5I0uf>y+@!Z|QnqlnhIqS%;v?<$r zLQJP3JQCBpCALagFBzq3v0Q8EjS{cvm0GP-swvt46C>*GgKW zrt4ZsZ)p{s*Y)P>wduvw3lqdvmU4SLyW6?t_RQ2`s&Y5zZlYs(g*_J^=qla-!xuWe zrgqE8UPj~J9m=sy8P3_s${4eZ$1@hb4?083{H<^QZ!4XB>leSd+W5zy z%1dJZ<>^HIv)%Uqx%KLgKRJm@YDIJqahhqO<4z{Q<#zEKjU<=7pS*S=v53ov_!yUq2`+0^`5z)K zzm4X9{*C{e*!+kfaYpCS2G@5M5?t2IKmJO@B@z3?pZxcU#3G{6Z<5P{ADN#^EzUj6 zk;E^5;Y(Q}OhgOcx$@+Pz(E1aYcSD2;JVoDOt(xdoC&$3ExCgPTJqB|yF*v3xf$h; z8-SdnV-cs={)@KCGe)VC&kpKyg2DTOL2maV2EY2}-@S4ggLTlPE!Z4vdFGnfLoR=0 zW;{O_YL@BsxSLNb%$m%1iWm=aS$s0Pqtt^Rn-z?IOfas!B1A^WzWHbWc`D<@M`Bzo zE))Iq-;%NgjY#ApwvL`1hrjimQyDEk5~JagxC!WHK8-mOLZ^5&?;TL_i`S5s(N-1SA3y0f~S_Kq4R!I0k_W({pLE?a8NmF}{FL_Cj<) ze$=J-0)Eg;bV0t$#rOig$A##E{Dt%J1$=XJ@df-{v+)IdN;B~V{58|@1$-b=OVe|U zk>7!Q3DeVmpPK#RjEx_8NCYGT?|%dierS3=wRr_!?VkE&F6Iu(_=*s-4)rQiGr_Nx zSy@>L&%I$k6Mjy7-tQY9UY%M<-M#Yg?g926Jgo z9G{Zwj!Ut1BFfHy+Xy=&D2E`O_q)O`{akthvirIRkdA)rKK>P90w2?h0v!o_)rB8 z$?sivVhTj_d1%BwdoVT7B;-A&k^TPAHg%|zC!{3gC<0@lHJA&;-V4jD=lcV1wNSvk zq1m$Y#&}jT&M0`0hu&kYqrf zh7^n@;ZWkC+qkO5XcErLfe{*YUX4NcB*`<62iU7(+#lW8L4J{*&x~gd>UyVzCHMsA zFXOR9fEx6%cpsaCgqVhi*@%7FB;(5d^sI=;eVU))1xFw=>x7W7TUTW{Ow-m&F zQ?}bXke(5CGVxd>ImRf9!QFOz8s3v^HlQ%8~3X!KoXg6E)q z(hZ1##vb&Ai72DM4(cO01&;ubj4RKD&BWu=#ik{qs{#dLP#qv(+W-;XC=NNb7{a%l zy`XnB1wGQ3tDjVn$M&y6ZTIoqr@dYfK`?sS#5D!t&9hB<*?UG4x7#7u-i-yd(7mp$xN>(Q+Yyt1=QR;fT-DZtiHJ=d~K{TL{ zq=`LVd^~8wvSQ4z9aN!W4A7Boal4Cg#4`J)A9wfQ{VPER?-s7B?vvECeLM_u46h=F zceUs^#ITr@Q1E=HS5Z7d_jC%A zFb1IzQIHBTJ%LjAx+=5;MT1L}3}H-`3|}_S`~5%J|LKKiQuvdHL_i`S5s(N-1SA3y zfkO!V>h{b+s&(b7_a?ffum@x0i(1YsFSBj3+OXjwUJJ1YB6+0X_Q5`YbdUbQQh*D} za=PM0SY(0+PrQ(dm-gHHeh(3ow^7JYU4x3m-T_DV@&j>Scn2y4%hX)e-7kdXgkNAe zyjTfn6M;4O1<95&X{*9M?OG^|8CM@-Eh)?#kL_{GN>2&IPnHW|)`Dg^ zTA@NEZ7M-DwW#T!i zB5Wf4>O&Y%eR}(LVT;0d3NQkwVEI9QU|P5cfmdLE23B~$P9(R;Dx~1nF^l9uGfWYW z8>U7Kw%F*>6KsqVIh2F-VIJ5V?4WV540o}NJk+8-OhuS0aCbP!<--$!)|#+gAxhVV z4RVMSwS=s%=eRcPl7&mG4Y&bCgex;)V?@>|8NnWhpE+<(8dOdwsOfpaYeWv8CVQ@Z z^W_(AV7pIY*>xNuO}NJpBuER(h`Im-D2(jT{&)Gnaec;LqlNzPR4*s_iEQ8XThDLa zD2saTs4mR&xD|;SP&GW@gxbJ?HBSaSEZ@M#8}LoG>A>+kQ^SoRTnurY8i&nmI&M_q zowco7H!7mSw(Fu;2-=bAghP{Dq+|+{Qvp`JW05Z?fT+E-wYhbp8bjnlUZf?ehCN*2 zlY*o;d=+SrAma6gzTxb_)((n4zqYfsaid1X#c=Yx@PvfoB;O&R?hgVf305NK7fl3o z;gSzr6+>?T#4L;Rq8BfXK`fWg|Lbs)INPv&Wg>=!)|MvmYbJ-%*lc zS|VhvGqlAQC4!^lxrnkwwb1^x@knw)jiD=)vcFxfwPOS*+LuJuiJhMxU|;iN+Td5% z|4)1@g+F;n1SA3y0f~S_Kq4R!kO)WwBmxoviGW04A_A|krDs#=&rdJFHAt%rR#_8M z)?!tL0;e7PA6Q%At}^cdLR0WcCK>G}vD%7)^uPy$HC}dCtm~p^IEnlC&Kk@KB_se1}7Qc%Wkg7m#lWoVHYTr|YA60GxBWmm6`CHLXm z5{cM;0=t-Cs@IqJTh08b^tb)IchbMDGxR zAj}bqoJAuX0w=G6Rh;T7`%DHtYz5nZV0)2O=EBzwz=i#v{>>Era9i%%afXj7ga0Qij7t=Gv1+Csd936TY|D$DJ2}rNaA;n0PQ0+HwT52SL|l zSChd+?%SjAV050?h6Vwj#NA|oupoOFA&f4Lga``~55)+h#5hP;qFAYz%q}j*0!{_x zqQGH?1BE3BjYh&G!FW`p#IwZ;yP7~>iI#oR(AZNw3}%nX(m5qOga@jw#=_YITOeN_^ol z7P4(jnehH~hWKq^k#~HEDwD|L@Na?qMcB(TjWuC3%ttL2J?Fv)??}1cI}g6|e$OT3 z`REV6ctLv@QEpt5j=mz@j=ofCQ>0C)L=<-Qolv6t_V@m7DZ8BRngg-V<=DICc~`Yh j>T-=S^ZvbN^-nUi$kI5`>Q9(-l=sjW1H zLRYeTy3XZ4|M~y_{O3RCd^L~WdF$+{%E98Ej~C>#A4wb~=0T)^Z}%Ll&y12$s$BlY z9$DjntYKSA=BvoM8m|eDeJk)E;%=wMQ_Hnd>!&DM=5gXj;S6gPlQfF0CEoE}9;UwM z^LU04?V>w~t&}Pi84Im2O1Kk+ZZg9vXk*2xA1-xLzsE<)JBc4Uyu~M<*JDDUnN~zV9TR6+i8Q%X}fazUSVe6ZQHr^1)2Ui{hSz zR)-D^W%nYNi_Dk@;Ch*NJU`%_zLjfO;DZg`ZL6T$XF zqy~BUjS!$gQ`Ycck?9Pej$Vh&eNl{^ncP zUiZj1AExyCDqofs3i^HD%|)5Dw{RnV#7@xh!<5IKDBojx3QB*Xy&D zE)P)+T#vBGBF7oTF?UNvd*BDK;JhdnGYUlW=paDP4-7DQFeh9#e{uoj(k@FQ7Q)y{ zm;^EOAzrA#3BIz{UxNOvly$9T4(`DYcx5nM$(C%NiT$2?S;$vlt4a+Ck;_Wgk#f3L zxCD-1U(~-CsM0{z2qaat9g}r=02|nrtg1Fd_V6x0#XA>npJAJ*Na%aWB740-=sPf{ zL}*Fq(dC{WVl-VI!$D25ZkqO!nR40XiN6$<$d8tyc&SW&%z-y3J3-{!-?2hh__R_t z?FHJjl;eZ#_xdoxFr8w{FbkJOaHGr$0$68SH7RqzVUh9;tu%q0>sU_&mQB+oCZ)1_ zDpSnh?VC%*fuy3E7K^(q;=@wLBiN@IDN|%@r)7 zzQ`{Xhbzmz-f3kCKEe522@Dc&wAO@{| zKaTo{fKo0;4CM#Wikw|7L60ov&}j>G!t@Z#b~T`7M1(}^Oiik{#7@$pMD(RB*E)tj z201DbtO_Ax4#0H9N`UI{8h$8a7(_F|ypKdtz+sny*pyiHV4@r|2i^tK2t$sUX9Wpn zgN?e3{*!7X=8s`9UqVgE3%L7` z=V6(3I}adS#e@^#^;winGX!CnORF=iig?%MeG0ZI^hK2JTectgDas5g!)8Ojo1(7e zBHR_53k2lc$!{f4j(A*TuI(xq?8C$Sx5S2J=WUk3N{G#cI}xVF_aswiw&4ZH2t>{g>~)h(2^ zt7fG(@h#AzXRrAGfZA*7w16XoGlxq2rEk{{T!R{AxH6bKc5-Ck9VhT%>c~g60v%Vt z5o6EX6QQ-pMBeB_b+>yao=`O;Ta`P0DYn2m)olIjck|n_^(jyi`+O-s9g-6JI5S!H z9bVY85lb88jj}!Q!VsH}9C$;psZ-77BWZtIHtV27Zk4tgF^*`G(mH2c>SIOr6GRK8 zH7{DA)1nO??C1;25gm~Pe1qbltcmB;tCaR@$NwmXp;JAK&wn@Bb{Hlo?R*$&)!1MR zbd+&F=lxD#C1hY*JrmEUQpx^H3vUYcb*kAnmzTF?zXD1-XJ4#yV&=T57E;aT+c2L0 z9Q=!5Tc?`seRr*H%XZ_u*v_rV8%UmpK*l3$?Z9|`L@n@0=|A?=RiCC)DxKM zyl@Xu6E&uE%=8x!-gPC<6_`i=h(-RP9s(iEd^Zkal_ zWjDGjy*7-D;9UIk4vj#kdIVnt>DmvUIfqvg&1jk=|QFNnIJp$_H$B#a{ zWjA^RQWk~6$Oul~{g^ll*J*wPPycFlL*ZynwGQ;VPN~Zg9q1!>o#sdI{99kyvKu{u zOXvrl1D;Hw-uv)NThHAeli6C$zfkX5H`i0xg+~kVYwBH({5AEiS-ef(8Z66pu$ex7 z>42_&h0NBG)UVaM4p|bKN_h0zKXa9Q*Nf@D)Vp4#o~3Og9_{W|J|f>Qe_9dBT~$4S ztWB<}zK)^`x~ekjm1e8eYSi23ac5;T+ReIYnvJSq8nt@cTz_X}RJXpgn#KVO?zFNy zH@bqNhc-MjY%9mFDpF8C!*;j~sW$qqPu$vN7iHL|tMZMu>9{k?uOVl{wQV~6&cy`S zAB)o5MqOs;b%aXSZM$XW$X=Z?v(5VXHq-la#n0v&_VXjWvt8GroO(L>Jp=8(W|-Q2 zUFgapIvKeeY#{SItjZjTCvtp`&1(4MNLu3u)U4<*ZX2_bM}Fm%*vBDh(#M(IkTGH3 zPk@7bs7#m2XQ=uD)fSPmYVwY|D3i47j$IUvHHKVNoqP3`*EA{(tL}JS%d1<>He%tH zW3&vT$+>A%jC!kHab3r$H=3?jtKpc)!0AxCQSn+rpCc}62eveNkUVTj+d&h?MnnhI zqX*L$W1Tx2zWu1+gE!^I(yBDc3U@Pxy3iBsjT}@HncFOOSk0or$O54xbUdj&03>kIJ z+Qjes4(;9X>?zP0!w7GSrzC8J2SL88B>*!9b_teJhO;os zs%qH|BD-9z!`2WrRT?#3ajI3TR%uin+iBOD+_pW$p_V~+Z@4gXf`QAk))||V#|^*+ zjvGx=iUdd(TtqJe+a~Uj1)L!g&=3+H$zfDtz`h8;Bw}qf8Um#BeAfkTL!r&QL5f54 zDe6KEky2?11e=HvGfM!S6cr#l&>sPNNfVHl1P)2~lw?QiO~ZKz8jBphatUt&!eALUVaken!1w@XA|O8i1_MF{27Q2z=;S^Gcw~eW zjf7w`w{d&0M9?H+tMg{A<($|QXO+=>C3j#Hfa&_ybTf#>qDcC~`fZYrs!QYW)Z>EJ9uZvLwN|l5YYP^&?=aZ4?ZE4a@qpIlxMO6zdMp%+8-U zTooP8#W9*Gp{?sqAW;e1N*E?Wg8_~e1q9-gBAT0@KRJK6mWhaf++-z|jU|^^q$v)N z83rVXB;7&kMk@eG$>v9A7iW(ju2Xa2PL2u)n^Gr%J|XHM^sjPt0-UzQLxnN3C3H(- za5ur2lGb{CQsu1~sEqi6jZ3o=U_oLW_+Bwn0B(aj)2xa~L(Laq0RpuM)j}Gi36LSX zpy@*RE`lj*MjgU85^_u9b3|KBM?fHn164h%1Sgx-!L_a+zd1=l=Va9fp}4W%u8FFq zn-M@J^CVmJs4%Ql{bFGvH&LJveO|I_sMYr|NO%nOhtHQ^BaET`Yk*!_h=x8!7(@NX zQ2$fF8M88e3ZkD>nT{}k%K z?{#sMgHxTtXX!9!M|XL-!yN+o$z6`vorn)4)#paE&yax!eff#Q1cIpIumxLQd~1g< zGxUcX@=biNt9DH5XyNlnP{`ABVlKWI63Z0rs_>Z%K1qpU+COT_^d%ZTOAtGrcLLz| z1BD^lgR?Ob5*-j>YfDHXpfJ9H(#^1ZSD-9^3n1-J@?UE0?4odM7jU5$g!^Am z-g+MvKf)iD*8^RY=e|fN><`FyZdJrw6ZB zg}12mugbBg?GAw)vpTOT($A4|!;`CXg@V2p_C2mBT}P`6o~&g5PY%nRYc7*%R&v=Z ucgUPC$Yia}rB@%5S@({X9PFzS)wHmehEI=X;CWTy4`-i!@=^6?rTz!;{SQ(A literal 0 HcmV?d00001 diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp index 2ba7701a50..fe2e807706 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp @@ -324,3 +324,86 @@ INSTANTIATE_TEST_SUITE_P( ParametrizedTemporaryDirectoryFixture, ValuesIn(rosbag2_test_common::kTestedStorageIDs) ); + +class GetServiceInfoTest : public Test +{ +public: + GetServiceInfoTest() {} + + virtual ~GetServiceInfoTest() = default; + + const std::string bags_path = std::string(_TEST_RESOURCES_DIR_PATH) + "/bags/"; +}; + +TEST_F(GetServiceInfoTest, only_topics_bag_test) { + const auto test_bag_file = rcpputils::fs::path(bags_path) / "only_topics.mcap"; + const std::string storage_id = "mcap"; + + rosbag2_cpp::Info info; + std::vector> ret_service_info; + + ASSERT_NO_THROW(ret_service_info = info.read_service_info(test_bag_file.string(), storage_id)); + + EXPECT_TRUE(ret_service_info.empty()); +} + +TEST_F(GetServiceInfoTest, only_services_bag_test) { + const auto test_bag_file = rcpputils::fs::path(bags_path) / "only_services.mcap"; + const std::string storage_id = "mcap"; + + rosbag2_cpp::Info info; + std::vector> ret_service_info; + + ASSERT_NO_THROW(ret_service_info = info.read_service_info(test_bag_file.string(), storage_id)); + + ASSERT_EQ(ret_service_info.size(), 1); + EXPECT_EQ(ret_service_info[0]->name, "/add_two_ints"); + EXPECT_EQ(ret_service_info[0]->type, "example_interfaces/srv/AddTwoInts"); + EXPECT_EQ(ret_service_info[0]->request_count, 6); + EXPECT_EQ(ret_service_info[0]->response_count, 6); + EXPECT_EQ(ret_service_info[0]->serialization_format, "cdr"); +} + +TEST_F(GetServiceInfoTest, topics_and_services_bag_test) { + const auto test_bag_file = rcpputils::fs::path(bags_path) / "topics_and_services.mcap"; + const std::string storage_id = "mcap"; + + rosbag2_cpp::Info info; + std::vector> ret_service_info; + + ASSERT_NO_THROW(ret_service_info = info.read_service_info(test_bag_file.string(), storage_id)); + + ASSERT_EQ(ret_service_info.size(), 2); + EXPECT_EQ(ret_service_info[0]->name, "/add_two_ints2"); + EXPECT_EQ(ret_service_info[0]->type, "example_interfaces/srv/AddTwoInts"); + EXPECT_EQ(ret_service_info[0]->request_count, 0); + EXPECT_EQ(ret_service_info[0]->response_count, 0); + EXPECT_EQ(ret_service_info[0]->serialization_format, "cdr"); + EXPECT_EQ(ret_service_info[1]->name, "/add_two_ints"); + EXPECT_EQ(ret_service_info[1]->type, "example_interfaces/srv/AddTwoInts"); + EXPECT_EQ(ret_service_info[1]->request_count, 3); + EXPECT_EQ(ret_service_info[1]->response_count, 3); + EXPECT_EQ(ret_service_info[1]->serialization_format, "cdr"); +} + +TEST_F(GetServiceInfoTest, topics_and_services_sqlite3_bag_test) { + const auto test_bag_file = rcpputils::fs::path(bags_path) / "topics_and_services.db3"; + const std::string storage_id = "sqlite3"; + + rosbag2_cpp::Info info; + std::vector> ret_service_info; + + ASSERT_NO_THROW(ret_service_info = info.read_service_info(test_bag_file.string(), storage_id)); + + ASSERT_EQ(ret_service_info.size(), 2); + EXPECT_EQ(ret_service_info[0]->name, "/add_two_ints"); + EXPECT_EQ(ret_service_info[0]->type, "example_interfaces/srv/AddTwoInts"); + EXPECT_EQ(ret_service_info[0]->request_count, 3); + EXPECT_EQ(ret_service_info[0]->response_count, 3); + EXPECT_EQ(ret_service_info[0]->serialization_format, "cdr"); + EXPECT_EQ(ret_service_info[1]->name, "/add_two_ints2"); + EXPECT_EQ(ret_service_info[1]->type, "example_interfaces/srv/AddTwoInts"); + EXPECT_EQ(ret_service_info[1]->request_count, 0); + EXPECT_EQ(ret_service_info[1]->response_count, 0); + EXPECT_EQ(ret_service_info[1]->serialization_format, "cdr"); +} From a78745b23b891a39b4fe44c7411ecdf5fbba8253 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 18 Oct 2023 14:42:11 +0800 Subject: [PATCH 04/35] Updated code on exclude parameters Refer to https://github.com/ros2/rosbag2/pull/1483 Signed-off-by: Barry Xu --- ros2bag/ros2bag/verb/record.py | 21 ++- .../rosbag2_transport/record_options.hpp | 5 +- .../src/rosbag2_transport/record_options.cpp | 7 +- .../src/rosbag2_transport/topic_filter.cpp | 26 ++- .../test_keyboard_controls.cpp | 2 +- .../test/rosbag2_transport/test_record.cpp | 16 +- .../rosbag2_transport/test_record_all.cpp | 7 +- .../test_record_all_ignore_leaf_topics.cpp | 2 +- ..._record_all_include_unpublished_topics.cpp | 6 +- .../test_record_all_no_discovery.cpp | 2 +- .../test_record_all_use_sim_time.cpp | 2 +- .../rosbag2_transport/test_record_options.cpp | 7 +- .../rosbag2_transport/test_record_regex.cpp | 155 +++++++++++++++++- .../test_record_services.cpp | 2 +- .../test/rosbag2_transport/test_rewrite.cpp | 2 +- .../rosbag2_transport/test_topic_filter.cpp | 65 ++++++-- 16 files changed, 263 insertions(+), 64 deletions(-) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index e7a1ed3ce9..6382d31056 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -79,12 +79,16 @@ def add_arguments(self, parser, cli_name): # noqa: D102 'Overrides --all, --all-topics and --all-services, applies on top of ' 'topics list and service list.') parser.add_argument( - '--exclude-topics', default='', - help='Exclude topics containing provided regular expression. ' + '--exclude-regex', default='', + help='Exclude topics and services containing provided regular expression. ' 'Works on top of --all, --all-topics, or --regex.') parser.add_argument( - '--exclude-services', default='', - help='Exclude services containing provided regular expression. ' + '--exclude-topics', type=str, metavar='Topic', nargs='+', + help='List of topics not being recorded. ' + 'Works on top of --all, --all-topics, or --regex.') + parser.add_argument( + '--exclude-services', type=str, metavar='ServiceName', nargs='+', + help='List of services not being recorded. ' 'Works on top of --all, --all-services, or --regex.') # Enable to record service @@ -212,6 +216,11 @@ def main(self, *, args): # noqa: D102 return print_error('Must specify only one option out of --all, --all-topics, ' 'topics or --regex') + if (args.exclude_regex and \ + not (args.regex or args.all or args.all_topics or args.all_services)): + return print_error('--exclude-regex argument requires either --all, --all, ' + '--all-topics, --all-services or --regex') + if args.exclude_topics and not (args.regex or args.all or args.all_topics): return print_error('--exclude-topics argument requires either --all, --all-topics ' 'or --regex') @@ -277,8 +286,10 @@ def main(self, *, args): # noqa: D102 record_options.topic_polling_interval = datetime.timedelta( milliseconds=args.polling_interval) record_options.regex = args.regex + record_options.exclude_regex = args.exclude_regex record_options.exclude_topics = args.exclude_topics - record_options.exclude_services = args.exclude_services + record_options.exclude_services = \ + convert_service_to_service_event_topic(args.exclude_services) record_options.node_prefix = NODE_NAME_PREFIX record_options.compression_mode = args.compression_mode record_options.compression_format = args.compression_format diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index ee181184df..04c86a7761 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -35,11 +35,12 @@ struct RecordOptions bool is_discovery_disabled = false; std::vector topics; std::vector services; // service event topic + std::vector exclude_topics; + std::vector exclude_services; // service event topic std::string rmw_serialization_format; std::chrono::milliseconds topic_polling_interval{100}; std::string regex = ""; - std::string exclude_topics = ""; - std::string exclude_services = ""; + std::string exclude_regex = ""; 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 701eb64fbb..d72ef358a1 100644 --- a/rosbag2_transport/src/rosbag2_transport/record_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/record_options.cpp @@ -35,6 +35,7 @@ Node convert::encode( 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_regex"] = record_options.exclude_regex; node["exclude_topics"] = record_options.exclude_topics; node["exclude_services"] = record_options.exclude_services; node["node_prefix"] = record_options.node_prefix; @@ -65,8 +66,10 @@ bool convert::decode( node, "topic_polling_interval", record_options.topic_polling_interval); optional_assign(node, "regex", record_options.regex); - optional_assign(node, "exclude_topics", record_options.exclude_topics); - optional_assign(node, "exclude_services", record_options.exclude_services); + optional_assign(node, "exclude_regex", record_options.exclude_regex); + 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/topic_filter.cpp b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp index 374156895f..2f09e86693 100644 --- a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp +++ b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp @@ -162,9 +162,8 @@ bool TopicFilter::take_topic( 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)) + topic_in_list(topic_name, record_options_.exclude_topics)) { return false; } @@ -177,6 +176,13 @@ bool TopicFilter::take_topic( return false; } + if (!record_options_.exclude_regex.empty()) { + std::regex exclude_regex(record_options_.exclude_regex); + if (std::regex_search(topic_name, exclude_regex)) { + return false; + } + } + if (!record_options_.include_hidden_topics && topic_is_hidden(topic_name)) { RCUTILS_LOG_WARN_ONCE_NAMED( ROSBAG2_TRANSPORT_PACKAGE_NAME, @@ -197,16 +203,15 @@ bool TopicFilter::take_topic( 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)) + service_in_list(topic_name, record_options_.exclude_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 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 @@ -214,6 +219,13 @@ bool TopicFilter::take_topic( { return false; } + + if (!record_options_.exclude_regex.empty()) { + std::regex exclude_regex(record_options_.exclude_regex); + if (std::regex_search(service_name, exclude_regex)) { + return false; + } + } } if (!allow_unknown_types_ && !type_is_known(topic_name, topic_type)) { diff --git a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp index 275b4c1048..84b8188c80 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp @@ -185,7 +185,7 @@ TEST_F(RecordIntegrationTestFixture, test_keyboard_controls) auto keyboard_handler = std::make_shared(); rosbag2_transport::RecordOptions record_options = - {true, false, false, {}, {}, "rmw_format", 100ms}; + {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 4915a975d1..da9a34d408 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -47,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, 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(); @@ -98,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, 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(); @@ -150,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, 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(); @@ -214,7 +214,7 @@ TEST_F(RecordIntegrationTestFixture, records_sensor_data) pub_manager.setup_publisher(topic, string_message, 2, rclcpp::SensorDataQoS()); rosbag2_transport::RecordOptions record_options = - {false, 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(); @@ -257,7 +257,7 @@ TEST_F(RecordIntegrationTestFixture, receives_latched_messages) pub_manager.run_publishers(); rosbag2_transport::RecordOptions record_options = - {false, 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(); @@ -302,7 +302,7 @@ TEST_F(RecordIntegrationTestFixture, mixed_qos_subscribes) { topic, profile_transient_local); rosbag2_transport::RecordOptions record_options = - {false, 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(); @@ -349,7 +349,7 @@ TEST_F(RecordIntegrationTestFixture, duration_and_noncompatibility_policies_mixe auto publisher_liveliness = create_pub(profile_liveliness); rosbag2_transport::RecordOptions record_options = - {false, 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(); @@ -387,7 +387,7 @@ TEST_F(RecordIntegrationTestFixture, write_split_callback_is_called) mock_writer.set_max_messages_per_file(5); rosbag2_transport::RecordOptions record_options = - {false, 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 be76ac4ce9..6680aa754b 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp @@ -50,7 +50,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are pub_manager.setup_publisher(string_topic, string_message, 2); rosbag2_transport::RecordOptions record_options = - {true, false, false, {}, {}, "rmw_format", 100ms}; + {true, false, false, {}, {}, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -100,7 +100,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_services_a "test_service_2"); rosbag2_transport::RecordOptions record_options = - {false, true, false, {}, {}, "rmw_format", 100ms}; + {false, true, false, {}, {}, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -140,8 +140,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_a 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"; + {true, true, false, {}, {}, {"rosout"}, {}, "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_ignore_leaf_topics.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp index 292cfbdd86..1422d831e5 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 @@ -52,7 +52,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_two_topics_ignore_l pub_manager.setup_publisher(string_topic, string_message, 2); rosbag2_transport::RecordOptions record_options = - {true, false, false, {}, {}, "rmw_format", 100ms}; + {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 644840d976..ce712aa73f 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 @@ -35,7 +35,7 @@ TEST_F(RecordIntegrationTestFixture, record_all_include_unpublished_false_ignore string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {}); rosbag2_transport::RecordOptions record_options = - {true, false, false, {}, {}, "rmw_format", 100ms}; + {true, false, false, {}, {}, {}, {}, "rmw_format", 100ms}; record_options.include_unpublished_topics = false; auto recorder = std::make_shared(writer_, storage_options_, record_options); recorder->record(); @@ -53,7 +53,7 @@ TEST_F(RecordIntegrationTestFixture, record_all_include_unpublished_true_include string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {}); rosbag2_transport::RecordOptions record_options = - {true, false, false, {}, {}, "rmw_format", 100ms}; + {true, false, false, {}, {}, {}, {}, "rmw_format", 100ms}; record_options.include_unpublished_topics = true; auto recorder = std::make_shared(writer_, storage_options_, record_options); recorder->record(); @@ -73,7 +73,7 @@ TEST_F( string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {}); rosbag2_transport::RecordOptions record_options = - {true, false, false, {}, {}, "rmw_format", 100ms}; + {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 845206cbeb..a33b355838 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 @@ -38,7 +38,7 @@ TEST_F(RecordIntegrationTestFixture, record_all_without_discovery_ignores_later_ string_message->string_value = "Hello World"; rosbag2_transport::RecordOptions record_options = - {true, false, true, {}, {}, "rmw_format", 100ms}; + {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 a2b9a9f192..0d9241aff2 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, 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 847f647afe..6793c1ea11 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp @@ -26,11 +26,12 @@ TEST(record_options, test_yaml_serialization) original.is_discovery_disabled = true; original.topics = {"topic", "other_topic"}; original.services = {"service", "other_service"}; + original.exclude_topics = {"exclude_topic1", "exclude_topic2"}; + original.exclude_services = {"exclude_service1", "exclude_service2"}; original.rmw_serialization_format = "cdr"; original.topic_polling_interval = std::chrono::milliseconds{200}; original.regex = "[xyz]/topic"; - original.exclude_topics = "*"; - original.exclude_services = "*"; + original.exclude_regex = "[x]/topic"; original.node_prefix = "prefix"; original.compression_mode = "stream"; original.compression_format = "h264"; @@ -53,6 +54,8 @@ TEST(record_options, test_yaml_serialization) CHECK(is_discovery_disabled); CHECK(topics); CHECK(services); + CHECK(exclude_topics); + CHECK(exclude_services); CHECK(rmw_serialization_format); #undef CHECK } diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp index 7af1a6a815..0d02cc8805 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp @@ -61,7 +61,7 @@ TEST_F(RecordIntegrationTestFixture, regex_topics_recording) ASSERT_FALSE(std::regex_match(b4, re)); rosbag2_transport::RecordOptions record_options = - {false, false, false, {}, {}, "rmw_format", 10ms}; + {false, false, false, {}, {}, {}, {}, "rmw_format", 10ms}; record_options.regex = regex; // TODO(karsten1987) Refactor this into publication manager @@ -102,7 +102,7 @@ TEST_F(RecordIntegrationTestFixture, regex_topics_recording) EXPECT_TRUE(recorded_topics.find(v1) != recorded_topics.end()); } -TEST_F(RecordIntegrationTestFixture, regex_and_exclude_topic_recording) +TEST_F(RecordIntegrationTestFixture, regex_and_exclude_regex_topic_recording) { auto test_string_messages = get_messages_strings(); auto test_array_messages = get_messages_arrays(); @@ -133,9 +133,9 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_topic_recording) ASSERT_TRUE(std::regex_match(e1, exclude)); rosbag2_transport::RecordOptions record_options = - {false, false, false, {}, {}, "rmw_format", 10ms}; + {false, false, false, {}, {}, {}, {}, "rmw_format", 10ms}; record_options.regex = regex; - record_options.exclude_topics = topics_regex_to_exclude; + record_options.exclude_regex = topics_regex_to_exclude; // TODO(karsten1987) Refactor this into publication manager @@ -179,7 +179,82 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_topic_recording) EXPECT_TRUE(recorded_topics.find(v2) != recorded_topics.end()); } -TEST_F(RecordIntegrationTestFixture, regex_and_exclude_service_recording) +TEST_F(RecordIntegrationTestFixture, regex_and_exclude_topic_topic_recording) +{ + auto test_string_messages = get_messages_strings(); + auto test_array_messages = get_messages_arrays(); + std::string regex = "/[a-z]+_nice(_.*)"; + std::string topics_exclude = "/quite_nice_namespace/but_it_is_excluded"; + + // matching topics - the only ones that should be recorded + std::string v1 = "/awesome_nice_topic"; + std::string v2 = "/still_nice_topic"; + + // excluded topics + std::string e1 = "/quite_nice_namespace/but_it_is_excluded"; + + // topics that shouldn't match + std::string b1 = "/numberslike1arenot_nice"; + std::string b2 = "/namespace_before/not_nice"; + + // checking the test data itself + std::regex re(regex); + ASSERT_TRUE(std::regex_match(v1, re)); + ASSERT_TRUE(std::regex_match(v2, re)); + ASSERT_FALSE(std::regex_match(b1, re)); + ASSERT_FALSE(std::regex_match(b2, re)); + + // this example matches both regexes - should be excluded + ASSERT_TRUE(std::regex_match(e1, re)); + ASSERT_TRUE(e1 == topics_exclude); + + rosbag2_transport::RecordOptions record_options = + {false, false, false, {}, {}, {}, {}, "rmw_format", 10ms}; + record_options.regex = regex; + record_options.exclude_topics.emplace_back(topics_exclude); + + // TODO(karsten1987) Refactor this into publication manager + rosbag2_test_common::PublicationManager pub_manager; + pub_manager.setup_publisher(v1, test_string_messages[0], 3); + pub_manager.setup_publisher(v2, test_string_messages[1], 3); + pub_manager.setup_publisher(b1, test_string_messages[0], 3); + pub_manager.setup_publisher(b2, test_string_messages[1], 3); + pub_manager.setup_publisher(e1, test_string_messages[0], 3); + + 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(v1.c_str())); + ASSERT_TRUE(pub_manager.wait_for_matched(v2.c_str())); + + pub_manager.run_publishers(); + + auto & writer = recorder->get_writer_handle(); + MockSequentialWriter & mock_writer = + static_cast(writer.get_implementation_handle()); + + size_t expected_messages = 3; + auto ret = rosbag2_test_common::wait_until_shutdown( + std::chrono::seconds(5), + [&mock_writer, &expected_messages]() { + return mock_writer.get_messages().size() >= expected_messages; + }); + auto recorded_messages = mock_writer.get_messages(); + // We may receive additional messages from rosout, it doesn't matter, + // as long as we have received at least as many total messages as we expect + EXPECT_TRUE(ret) << "failed to capture expected messages in time"; + EXPECT_THAT(recorded_messages, SizeIs(Ge(expected_messages))); + + auto recorded_topics = mock_writer.get_topics(); + EXPECT_THAT(recorded_topics, SizeIs(2)); + EXPECT_TRUE(recorded_topics.find(v1) != recorded_topics.end()); + EXPECT_TRUE(recorded_topics.find(v2) != recorded_topics.end()); +} + +TEST_F(RecordIntegrationTestFixture, regex_and_exclude_regex_service_recording) { std::string regex = "/[a-z]+_nice(_.*)"; std::string services_regex_to_exclude = "/[a-z]+_nice_[a-z]+/(.*)"; @@ -188,6 +263,72 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_service_recording) std::string v1 = "/awesome_nice_service"; std::string v2 = "/still_nice_service"; + // excluded service + 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_regex = 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()); +} + +TEST_F(RecordIntegrationTestFixture, regex_and_exclude_service_service_recording) +{ + std::string regex = "/[a-z]+_nice(_.*)"; + std::string services_exclude = "/quite_nice_namespace/but_it_is_excluded/_service_event"; + + // 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"; @@ -196,9 +337,9 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_service_recording) std::string b2 = "/namespace_before/not_nice"; rosbag2_transport::RecordOptions record_options = - {false, false, false, {}, {}, "rmw_format", 10ms}; + {false, false, false, {}, {}, {}, {}, "rmw_format", 10ms}; record_options.regex = regex; - record_options.exclude_services = services_regex_to_exclude; + record_options.exclude_services.emplace_back(services_exclude); auto service_manager_v1 = std::make_shared>(v1); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp index 32b78f27a6..2db596c653 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, 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 4a951107dd..8086787c5e 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp @@ -189,7 +189,7 @@ TEST_P(TestRewrite, test_filter_split) { storage_opts.storage_id = storage_id_; rosbag2_transport::RecordOptions rec_opts; rec_opts.all_topics = true; - rec_opts.exclude_topics = "basic"; + rec_opts.exclude_regex = "basic"; output_bags_.push_back({storage_opts, rec_opts}); } { diff --git a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp index c22d2047f1..c93958f42d 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp @@ -189,10 +189,27 @@ TEST(TestTopicFilter, filter_topics) { } } +TEST_F(RegexFixture, regex_all_topics_and_exclude_regex) +{ + rosbag2_transport::RecordOptions record_options; + record_options.exclude_regex = "/inv.*"; + 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(3)); + for (const auto & topic : {"/planning", "/localization", "/status"}) { + EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()); + } +} + TEST_F(RegexFixture, regex_all_topics_and_exclude_topics) { rosbag2_transport::RecordOptions record_options; - record_options.exclude_topics = "/inv.*"; + record_options.exclude_topics = { + "/invalid_topic", + "/invalidated_topic", + "/invisible"}; record_options.all_topics = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_); @@ -203,10 +220,25 @@ TEST_F(RegexFixture, regex_all_topics_and_exclude_topics) } } +TEST_F(RegexFixture, regex_all_services_and_exclude_regex) +{ + rosbag2_transport::RecordOptions record_options; + record_options.exclude_regex = "/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_services_and_exclude_services) { rosbag2_transport::RecordOptions record_options; - record_options.exclude_services = "/inv.*"; + record_options.exclude_services = { + "/invalid_service/_service_event", + "/invalidated_service/_service_event" + }; record_options.all_services = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_); @@ -218,10 +250,9 @@ TEST_F(RegexFixture, regex_all_services_and_exclude_services) 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; + record_options.exclude_regex = "/inv.*"; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_); @@ -233,45 +264,44 @@ TEST_F(RegexFixture, regex_all_topics_all_services_and_exclude_topics_and_servic } } -TEST_F(RegexFixture, regex_filter_exclude_topics) +TEST_F(RegexFixture, regex_filter_exclude_regex) { rosbag2_transport::RecordOptions record_options; record_options.regex = "/invalid.*"; - record_options.exclude_topics = ".invalidated.*"; // Only affect topics + record_options.exclude_regex = ".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_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()); - EXPECT_TRUE(filtered_topics.find("/invalidated_service/_service_event") != filtered_topics.end()); } -TEST_F(RegexFixture, regex_filter_exclude_services) +TEST_F(RegexFixture, regex_filter_exclude_topics) { rosbag2_transport::RecordOptions record_options; record_options.regex = "/invalid.*"; - record_options.exclude_services = ".invalidated.*"; // Only affect services + record_options.exclude_topics = {"/invalidated_topic"}; 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()); + EXPECT_TRUE(filtered_topics.find("/invalidated_service/_service_event") != filtered_topics.end()); } -TEST_F(RegexFixture, regex_filter_exclude_topics_and_services) +TEST_F(RegexFixture, regex_filter_exclude_services) { rosbag2_transport::RecordOptions record_options; record_options.regex = "/invalid.*"; - record_options.exclude_topics = ".invalidated.*"; - record_options.exclude_services = ".invalidated.*"; + record_options.exclude_services = {"/invalidated_service/_service_event"}; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_); - EXPECT_THAT(filtered_topics, SizeIs(2)); + 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()); } @@ -296,7 +326,6 @@ TEST_F(RegexFixture, regex_all_topics_and_filter) rosbag2_transport::RecordOptions record_options; record_options.regex = "/status"; 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)); @@ -343,9 +372,9 @@ TEST_F(RegexFixture, do_not_print_warning_about_unknown_types_if_topic_is_not_se TEST_F(RegexFixture, regex_all_services_and_filter) { rosbag2_transport::RecordOptions record_options; - record_options.regex = "/status"; + record_options.regex = "/no_exist_service"; 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)); From c95335b6382240735972ead9d273264203567557 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 18 Oct 2023 15:00:44 +0800 Subject: [PATCH 05/35] Fix flake8 errors Signed-off-by: Barry Xu --- ros2bag/ros2bag/verb/record.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 6382d31056..2b034814a8 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -216,8 +216,8 @@ def main(self, *, args): # noqa: D102 return print_error('Must specify only one option out of --all, --all-topics, ' 'topics or --regex') - if (args.exclude_regex and \ - not (args.regex or args.all or args.all_topics or args.all_services)): + if (args.exclude_regex and + not (args.regex or args.all or args.all_topics or args.all_services)): return print_error('--exclude-regex argument requires either --all, --all, ' '--all-topics, --all-services or --regex') From f984bd231e6a9367b930fff3e4e3a58a9f76327f Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 19 Oct 2023 09:11:21 +0800 Subject: [PATCH 06/35] Update code to remove hard-coded values to make the code more readable Signed-off-by: Barry Xu --- rosbag2_py/src/rosbag2_py/format_service_info.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.cpp b/rosbag2_py/src/rosbag2_py/format_service_info.cpp index 0c1366bee9..aec57a0550 100644 --- a/rosbag2_py/src/rosbag2_py/format_service_info.cpp +++ b/rosbag2_py/src/rosbag2_py/format_service_info.cpp @@ -21,9 +21,10 @@ format_service_info( std::vector> & service_info_list) { std::stringstream info_stream; - int indentation_spaces = 21; + const std::string service_info_string = "Service information: "; + auto indentation_spaces = service_info_string.size(); info_stream << "Service: " << service_info_list.size() << std::endl; - info_stream << "Service information: "; + info_stream << service_info_string; if (service_info_list.empty()) { return info_stream.str(); From 0576e0dcc07c81ebed81fd5f3357292090d35fed Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 19 Oct 2023 13:40:21 +0800 Subject: [PATCH 07/35] Fix a bug on exclude parameter Signed-off-by: Barry Xu --- ros2bag/ros2bag/verb/record.py | 2 +- rosbag2_py/src/rosbag2_py/_transport.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 2b034814a8..6442cdf16b 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -287,7 +287,7 @@ def main(self, *, args): # noqa: D102 milliseconds=args.polling_interval) record_options.regex = args.regex record_options.exclude_regex = args.exclude_regex - record_options.exclude_topics = args.exclude_topics + record_options.exclude_topics = args.exclude_topics if args.exclude_topics else [] record_options.exclude_services = \ convert_service_to_service_event_topic(args.exclude_services) record_options.node_prefix = NODE_NAME_PREFIX diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index cf2831f9bb..e36a1fb738 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -356,6 +356,7 @@ PYBIND11_MODULE(_transport, m) { .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_regex", &RecordOptions::exclude_regex) .def_readwrite("exclude_topics", &RecordOptions::exclude_topics) .def_readwrite("exclude_services", &RecordOptions::exclude_services) .def_readwrite("node_prefix", &RecordOptions::node_prefix) From ac5112a2e3ddab7908c52d2ac1697a4b22cb3d8e Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 24 Oct 2023 10:05:36 +0800 Subject: [PATCH 08/35] Update codes on rosbag2 QoS for service event topic Signed-off-by: Barry Xu --- .../src/rosbag2_transport/qos.cpp | 234 ++++++++++++++++++ .../src/rosbag2_transport/recorder.cpp | 6 - 2 files changed, 234 insertions(+), 6 deletions(-) create mode 100644 rosbag2_transport/src/rosbag2_transport/qos.cpp diff --git a/rosbag2_transport/src/rosbag2_transport/qos.cpp b/rosbag2_transport/src/rosbag2_transport/qos.cpp new file mode 100644 index 0000000000..4233a1862f --- /dev/null +++ b/rosbag2_transport/src/rosbag2_transport/qos.cpp @@ -0,0 +1,234 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// 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 "rosbag2_cpp/service_utils.hpp" +#include "rosbag2_transport/qos.hpp" +#include "logging.hpp" + +namespace +{ +/** + * The following constants were the "Inifinity" value returned by RMW implementations before + * the introduction of RMW_DURATION_INFINITE and associated RMW fixes + * RMW: https://github.com/ros2/rmw/pull/301 + * Fast-DDS: https://github.com/ros2/rmw_fastrtps/pull/515 + * CycloneDDS: https://github.com/ros2/rmw_cyclonedds/pull/288 + * RTI Connext: https://github.com/ros2/rmw_connext/pull/491 + * + * These values exist in bags recorded in Foxy, they need to be translated to RMW_DURATION_INFINITE + * to be consistently understood for playback. + * With those values, if a bag is played back in a different implementation than it was recorded, + * the publishers will fail to be created with an error indicating an invalid QoS value.. + */ +static const rmw_time_t RMW_CYCLONEDDS_FOXY_INFINITE = rmw_time_from_nsec(0x7FFFFFFFFFFFFFFFll); +static const rmw_time_t RMW_FASTRTPS_FOXY_INFINITE {0x7FFFFFFFll, 0xFFFFFFFFll}; +static const rmw_time_t RMW_CONNEXT_FOXY_INFINITE {0x7FFFFFFFll, 0x7FFFFFFFll}; +} // namespace + +namespace YAML +{ +Node convert::encode(const rmw_time_t & time) +{ + Node node; + node["sec"] = time.sec; + node["nsec"] = time.nsec; + return node; +} + +bool convert::decode(const Node & node, rmw_time_t & time) +{ + time.sec = node["sec"].as(); + time.nsec = node["nsec"].as(); + if ( + rmw_time_equal(time, RMW_CYCLONEDDS_FOXY_INFINITE) || + rmw_time_equal(time, RMW_FASTRTPS_FOXY_INFINITE) || + rmw_time_equal(time, RMW_CONNEXT_FOXY_INFINITE)) + { + time = RMW_DURATION_INFINITE; + } + return true; +} + +Node convert::encode(const rosbag2_transport::Rosbag2QoS & qos) +{ + const auto & p = qos.get_rmw_qos_profile(); + Node node; + node["history"] = static_cast(p.history); + node["depth"] = p.depth; + node["reliability"] = static_cast(p.reliability); + node["durability"] = static_cast(p.durability); + node["deadline"] = p.deadline; + node["lifespan"] = p.lifespan; + node["liveliness"] = static_cast(p.liveliness); + node["liveliness_lease_duration"] = p.liveliness_lease_duration; + node["avoid_ros_namespace_conventions"] = p.avoid_ros_namespace_conventions; + return node; +} + +bool convert::decode( + const Node & node, rosbag2_transport::Rosbag2QoS & qos) +{ + auto history = static_cast(node["history"].as()); + auto reliability = static_cast(node["reliability"].as()); + auto durability = static_cast(node["durability"].as()); + auto liveliness = static_cast(node["liveliness"].as()); + + qos + .keep_last(node["depth"].as()) + .history(history) + .reliability(reliability) + .durability(durability) + .deadline(node["deadline"].as()) + .lifespan(node["lifespan"].as()) + .liveliness(liveliness) + .liveliness_lease_duration(node["liveliness_lease_duration"].as()) + .avoid_ros_namespace_conventions(node["avoid_ros_namespace_conventions"].as()); + return true; +} +} // namespace YAML + +namespace rosbag2_transport +{ +Rosbag2QoS Rosbag2QoS::adapt_request_to_offers( + const std::string & topic_name, const std::vector & endpoints) +{ + if (endpoints.empty()) { + return Rosbag2QoS{}; + } + size_t num_endpoints = endpoints.size(); + size_t reliability_reliable_endpoints_count = 0; + size_t durability_transient_local_endpoints_count = 0; + for (const auto & endpoint : endpoints) { + const auto & profile = endpoint.qos_profile().get_rmw_qos_profile(); + if (profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) { + reliability_reliable_endpoints_count++; + } + if (profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { + durability_transient_local_endpoints_count++; + } + } + + bool service_event_topic = + rosbag2_cpp::is_service_event_topic(topic_name, endpoints[0].topic_type()); + + // We set policies in order as defined in rmw_qos_profile_t + Rosbag2QoS request_qos{}; + // Policy: history, depth + // History does not affect compatibility + + // Policy: reliability + if (reliability_reliable_endpoints_count == num_endpoints) { + request_qos.reliable(); + } else { + if (reliability_reliable_endpoints_count > 0) { + ROSBAG2_TRANSPORT_LOG_WARN_STREAM( + "Some, but not all, publishers on topic \"" << topic_name << "\" " + "are offering RMW_QOS_POLICY_RELIABILITY_RELIABLE. " + "Falling back to RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT " + "as it will connect to all publishers. " + "Some messages from Reliable publishers could be dropped."); + } + request_qos.best_effort(); + } + + // Policy: durability + // If all publishers offer transient_local, we can request it and receive latched messages + // + // If topic is for service event, rosbag doesn't want to receive latched messages (such as + // last response message). So only use durability_volatile. + if (!service_event_topic && durability_transient_local_endpoints_count == num_endpoints) { + request_qos.transient_local(); + } else { + if (!service_event_topic && durability_transient_local_endpoints_count > 0) { + ROSBAG2_TRANSPORT_LOG_WARN_STREAM( + "Some, but not all, publishers on topic \"" << topic_name << "\" " + "are offering RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL. " + "Falling back to RMW_QOS_POLICY_DURABILITY_VOLATILE " + "as it will connect to all publishers. " + "Previously-published latched messages will not be retrieved."); + } + request_qos.durability_volatile(); + } + // Policy: deadline + // Deadline does not affect delivery of messages, + // and we do not record Deadline"Missed events. + // We can always use unspecified deadline, which will be compatible with all publishers. + + // Policy: lifespan + // Lifespan does not affect compatibiliy + + // Policy: liveliness, liveliness_lease_duration + // Liveliness does not affect delivery of messages, + // and we do not record LivelinessChanged events. + // We can always use unspecified liveliness, which will be compatible with all publishers. + return request_qos; +} + +namespace +{ +bool operator==(const rmw_time_t & lhs, const rmw_time_t & rhs) +{ + return lhs.sec == rhs.sec && lhs.nsec == rhs.nsec; +} + +/** Check if all QoS profiles in the vector are identical when only looking at + * policies that affect compatibility. + * This means it excludes history and lifespan from the equality check. + */ +bool all_profiles_effectively_same(const std::vector & profiles) +{ + auto iterator = profiles.begin(); + const auto p_ref = iterator->get_rmw_qos_profile(); + iterator++; + for (; iterator != profiles.end(); iterator++) { + const auto p_next = iterator->get_rmw_qos_profile(); + bool compatibility_equals_previous = ( + // excluding history + p_ref.reliability == p_next.reliability && + p_ref.durability == p_next.durability && + p_ref.deadline == p_next.deadline && + // excluding lifespan + p_ref.liveliness == p_next.liveliness && + p_ref.liveliness_lease_duration == p_next.liveliness_lease_duration + ); + if (!compatibility_equals_previous) { + return false; + } + } + return true; +} +} // unnamed namespace + +Rosbag2QoS Rosbag2QoS::adapt_offer_to_recorded_offers( + const std::string & topic_name, const std::vector & profiles) +{ + if (profiles.empty()) { + return Rosbag2QoS{}; + } + if (all_profiles_effectively_same(profiles)) { + auto result = profiles[0]; + return result.default_history(); + } + + ROSBAG2_TRANSPORT_LOG_WARN_STREAM( + "Not all original publishers on topic " << topic_name << " offered the same QoS profiles. " + "Rosbag2 cannot yet choose an adapted profile to offer for this mixed case. " + "Falling back to the rosbag2_transport default publisher offer."); + return Rosbag2QoS{}; +} +} // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 72d8cf58c1..b1b3dee19d 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -478,12 +478,6 @@ void RecorderImpl::subscribe_topic(const rosbag2_storage::TopicMetadata & topic) rosbag2_storage::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}); From 6b206b8a56f7c667de06b6b9102522328dfaa718 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 24 Oct 2023 17:22:30 +0800 Subject: [PATCH 09/35] Address the second round of review comments Signed-off-by: Barry Xu --- ros2bag/ros2bag/verb/record.py | 2 +- .../src/rosbag2_transport/topic_filter.cpp | 56 ++++++++++--------- .../rosbag2_transport/test_topic_filter.cpp | 1 + 3 files changed, 33 insertions(+), 26 deletions(-) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 6442cdf16b..a96c5a29d3 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -218,7 +218,7 @@ def main(self, *, args): # noqa: D102 if (args.exclude_regex and not (args.regex or args.all or args.all_topics or args.all_services)): - return print_error('--exclude-regex argument requires either --all, --all, ' + return print_error('--exclude-regex argument requires either --all, ' '--all-topics, --all-services or --regex') if args.exclude_topics and not (args.regex or args.all or args.all_topics): diff --git a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp index 2f09e86693..738d74024c 100644 --- a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp +++ b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp @@ -158,8 +158,19 @@ bool TopicFilter::take_topic( return false; } - if (!record_options_.topics.empty() && !topic_in_list(topic_name, record_options_.topics)) { - return false; + if (!record_options_.all_topics) { + // Not in include topic list + if (record_options_.topics.empty() || !topic_in_list(topic_name, record_options_.topics)) { + // Not match include regex + if (!record_options_.regex.empty()) { + std::regex include_regex(record_options_.regex); + if (!std::regex_search(topic_name, include_regex)) { + return false; + } + } else { + return false; + } + } } if (!record_options_.exclude_topics.empty() && @@ -168,14 +179,6 @@ bool TopicFilter::take_topic( 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; - } - if (!record_options_.exclude_regex.empty()) { std::regex exclude_regex(record_options_.exclude_regex); if (std::regex_search(topic_name, exclude_regex)) { @@ -197,10 +200,24 @@ bool TopicFilter::take_topic( 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); + + if (!record_options_.all_services) { + // Not in include service list + if (record_options_.services.empty() || + !service_in_list(topic_name, record_options_.services)) + { + // Not match include regex + if (!record_options_.regex.empty()) { + std::regex include_regex(record_options_.regex); + if (!std::regex_search(service_name, include_regex)) { + return false; + } + } else { + return false; + } + } } if (!record_options_.exclude_services.empty() && @@ -209,17 +226,6 @@ bool TopicFilter::take_topic( 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 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_.exclude_regex.empty()) { std::regex exclude_regex(record_options_.exclude_regex); if (std::regex_search(service_name, exclude_regex)) { diff --git a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp index c93958f42d..30b9820853 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp @@ -54,6 +54,7 @@ TEST(TestTopicFilter, filter_hidden_topics) { { rosbag2_transport::RecordOptions record_options; + record_options.all_topics = true; record_options.include_hidden_topics = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types); From cab5b2ed6798e4ecae18406deb7970edfe3ecf5f Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 26 Oct 2023 15:03:35 +0800 Subject: [PATCH 10/35] Updated info test on getting service info Signed-off-by: Barry Xu --- rosbag2_cpp/CMakeLists.txt | 2 - rosbag2_cpp/test/rosbag2_cpp/test_info.cpp | 83 ------ rosbag2_tests/CMakeLists.txt | 12 + .../test_rosbag2_info_get_service_info.cpp | 246 ++++++++++++++++++ 4 files changed, 258 insertions(+), 85 deletions(-) create mode 100644 rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_get_service_info.cpp diff --git a/rosbag2_cpp/CMakeLists.txt b/rosbag2_cpp/CMakeLists.txt index 953495e1ea..3ac49a2366 100644 --- a/rosbag2_cpp/CMakeLists.txt +++ b/rosbag2_cpp/CMakeLists.txt @@ -138,8 +138,6 @@ if(BUILD_TESTING) find_package(rosbag2_test_msgdefs REQUIRED) ament_lint_auto_find_test_dependencies() - add_definitions(-D_TEST_RESOURCES_DIR_PATH="${CMAKE_CURRENT_SOURCE_DIR}/test/${PROJECT_NAME}") - add_library( converter_test_plugins SHARED diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp index fe2e807706..2ba7701a50 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_info.cpp @@ -324,86 +324,3 @@ INSTANTIATE_TEST_SUITE_P( ParametrizedTemporaryDirectoryFixture, ValuesIn(rosbag2_test_common::kTestedStorageIDs) ); - -class GetServiceInfoTest : public Test -{ -public: - GetServiceInfoTest() {} - - virtual ~GetServiceInfoTest() = default; - - const std::string bags_path = std::string(_TEST_RESOURCES_DIR_PATH) + "/bags/"; -}; - -TEST_F(GetServiceInfoTest, only_topics_bag_test) { - const auto test_bag_file = rcpputils::fs::path(bags_path) / "only_topics.mcap"; - const std::string storage_id = "mcap"; - - rosbag2_cpp::Info info; - std::vector> ret_service_info; - - ASSERT_NO_THROW(ret_service_info = info.read_service_info(test_bag_file.string(), storage_id)); - - EXPECT_TRUE(ret_service_info.empty()); -} - -TEST_F(GetServiceInfoTest, only_services_bag_test) { - const auto test_bag_file = rcpputils::fs::path(bags_path) / "only_services.mcap"; - const std::string storage_id = "mcap"; - - rosbag2_cpp::Info info; - std::vector> ret_service_info; - - ASSERT_NO_THROW(ret_service_info = info.read_service_info(test_bag_file.string(), storage_id)); - - ASSERT_EQ(ret_service_info.size(), 1); - EXPECT_EQ(ret_service_info[0]->name, "/add_two_ints"); - EXPECT_EQ(ret_service_info[0]->type, "example_interfaces/srv/AddTwoInts"); - EXPECT_EQ(ret_service_info[0]->request_count, 6); - EXPECT_EQ(ret_service_info[0]->response_count, 6); - EXPECT_EQ(ret_service_info[0]->serialization_format, "cdr"); -} - -TEST_F(GetServiceInfoTest, topics_and_services_bag_test) { - const auto test_bag_file = rcpputils::fs::path(bags_path) / "topics_and_services.mcap"; - const std::string storage_id = "mcap"; - - rosbag2_cpp::Info info; - std::vector> ret_service_info; - - ASSERT_NO_THROW(ret_service_info = info.read_service_info(test_bag_file.string(), storage_id)); - - ASSERT_EQ(ret_service_info.size(), 2); - EXPECT_EQ(ret_service_info[0]->name, "/add_two_ints2"); - EXPECT_EQ(ret_service_info[0]->type, "example_interfaces/srv/AddTwoInts"); - EXPECT_EQ(ret_service_info[0]->request_count, 0); - EXPECT_EQ(ret_service_info[0]->response_count, 0); - EXPECT_EQ(ret_service_info[0]->serialization_format, "cdr"); - EXPECT_EQ(ret_service_info[1]->name, "/add_two_ints"); - EXPECT_EQ(ret_service_info[1]->type, "example_interfaces/srv/AddTwoInts"); - EXPECT_EQ(ret_service_info[1]->request_count, 3); - EXPECT_EQ(ret_service_info[1]->response_count, 3); - EXPECT_EQ(ret_service_info[1]->serialization_format, "cdr"); -} - -TEST_F(GetServiceInfoTest, topics_and_services_sqlite3_bag_test) { - const auto test_bag_file = rcpputils::fs::path(bags_path) / "topics_and_services.db3"; - const std::string storage_id = "sqlite3"; - - rosbag2_cpp::Info info; - std::vector> ret_service_info; - - ASSERT_NO_THROW(ret_service_info = info.read_service_info(test_bag_file.string(), storage_id)); - - ASSERT_EQ(ret_service_info.size(), 2); - EXPECT_EQ(ret_service_info[0]->name, "/add_two_ints"); - EXPECT_EQ(ret_service_info[0]->type, "example_interfaces/srv/AddTwoInts"); - EXPECT_EQ(ret_service_info[0]->request_count, 3); - EXPECT_EQ(ret_service_info[0]->response_count, 3); - EXPECT_EQ(ret_service_info[0]->serialization_format, "cdr"); - EXPECT_EQ(ret_service_info[1]->name, "/add_two_ints2"); - EXPECT_EQ(ret_service_info[1]->type, "example_interfaces/srv/AddTwoInts"); - EXPECT_EQ(ret_service_info[1]->request_count, 0); - EXPECT_EQ(ret_service_info[1]->response_count, 0); - EXPECT_EQ(ret_service_info[1]->serialization_format, "cdr"); -} diff --git a/rosbag2_tests/CMakeLists.txt b/rosbag2_tests/CMakeLists.txt index 5ac04d58dc..664e6ab747 100644 --- a/rosbag2_tests/CMakeLists.txt +++ b/rosbag2_tests/CMakeLists.txt @@ -120,6 +120,18 @@ if(BUILD_TESTING) ${test_msgs_TARGETS} ) endif() + + ament_add_gmock(test_rosbag2_info_get_service_info + test/rosbag2_tests/test_rosbag2_info_get_service_info.cpp + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + if(TARGET test_rosbag2_info_get_service_info) + target_link_libraries(test_rosbag2_info_get_service_info + rosbag2_cpp::rosbag2_cpp + rosbag2_storage::rosbag2_storage + rosbag2_test_common::rosbag2_test_common + ${test_msgs_TARGETS} + ) + endif() endif() ament_package() diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_get_service_info.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_get_service_info.cpp new file mode 100644 index 0000000000..6cac3813a6 --- /dev/null +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_get_service_info.cpp @@ -0,0 +1,246 @@ +// 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 "rosbag2_cpp/info.hpp" +#include "rosbag2_cpp/writer.hpp" + +#include "rosbag2_test_common/client_manager.hpp" +#include "rosbag2_test_common/process_execution_helpers.hpp" +#include "rosbag2_test_common/publication_manager.hpp" +#include "rosbag2_test_common/temporary_directory_fixture.hpp" + +#include "test_msgs/message_fixtures.hpp" +#include "test_msgs/msg/basic_types.hpp" +#include "test_msgs/srv/basic_types.hpp" + +class GetServiceInfoTest : public rosbag2_test_common::TemporaryDirectoryFixture +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + } + + void TearDown() override + { + rclcpp::shutdown(); + } + + rcpputils::fs::path get_bag_path() + { + return rcpputils::fs::path(temporary_dir_path_) / + UnitTest::GetInstance()->current_test_info()->name(); + } + + const std::string get_bag_file_name(int split_index, std::string storage_id) + { + if (storage_id == "mcap") { + return std::string(UnitTest::GetInstance()->current_test_info()->name()) + + "_" + std::to_string(split_index) + ".mcap"; + } else if (storage_id == "sqlite3") { + return std::string(UnitTest::GetInstance()->current_test_info()->name()) + + "_" + std::to_string(split_index) + ".db3"; + } else { + throw std::runtime_error("Invalid storage id : \"" + storage_id + "\""); + } + } + + void wait_for_storage_file( + std::string file_path, + std::chrono::duration timeout = std::chrono::seconds(10)) + { + auto check_file_path = rcpputils::fs::path(file_path); + const auto start_time = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start_time < timeout) { + if (check_file_path.exists()) { + return; + } + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + ASSERT_EQ(check_file_path.exists(), true) + << "Could not find storage file: \"" << check_file_path.string() << "\""; + } + + void topics_and_services_bag_test(std::string storage_id); +}; + +TEST_F(GetServiceInfoTest, only_topics_bag_test) { + const std::string storage_id = "mcap"; + const auto bag_path = get_bag_path(); + + { + // Create an empty bag with default storage + rosbag2_cpp::Writer writer; + rosbag2_storage::StorageOptions storage_options; + storage_options.storage_id = storage_id; + storage_options.uri = bag_path.string(); + writer.open(storage_options); + test_msgs::msg::BasicTypes msg; + writer.write(msg, "test_topic", rclcpp::Time{}); + } + std::string first_storage_file_path; + { + rosbag2_storage::MetadataIo metadata_io; + auto metadata = metadata_io.read_metadata(bag_path.string()); + first_storage_file_path = (bag_path / metadata.relative_file_paths[0]).string(); + } + + rosbag2_cpp::Info info; + std::vector> ret_service_info; + + ASSERT_NO_THROW(ret_service_info = info.read_service_info(first_storage_file_path, storage_id)); + + EXPECT_TRUE(ret_service_info.empty()); +} + +TEST_F(GetServiceInfoTest, only_services_bag_test) { + const std::string storage_id = "mcap"; + const auto bag_path = get_bag_path(); + const std::string record_cmd = "ros2 bag record --all-services -o " + bag_path.string(); + std::string bag_filename; + + ASSERT_NO_THROW(bag_filename = get_bag_file_name(0, storage_id)); + + auto service_client_manager = + std::make_shared>( + "test_service"); + + if (!service_client_manager->check_service_ready()) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + ASSERT_TRUE(service_client_manager->check_service_ready()); + } + + auto record_process = start_execution(record_cmd); + auto cleanup_process_handle = rcpputils::make_scope_exit( + [record_process]() { + stop_execution(record_process); + }); + + wait_for_storage_file(bag_path.string() + "/" + bag_filename); + + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + ASSERT_TRUE(service_client_manager->send_request()); + ASSERT_TRUE(service_client_manager->send_request()); + ASSERT_TRUE(service_client_manager->send_request()); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + stop_execution(record_process); + cleanup_process_handle.cancel(); + + rosbag2_cpp::Info info; + std::vector> ret_service_info; + + ASSERT_NO_THROW( + ret_service_info = + info.read_service_info(bag_path.string() + "/" + bag_filename, storage_id)); + + ASSERT_EQ(ret_service_info.size(), 1); + EXPECT_EQ(ret_service_info[0]->name, "/test_service"); + EXPECT_EQ(ret_service_info[0]->type, "test_msgs/srv/BasicTypes"); + EXPECT_EQ(ret_service_info[0]->request_count, 3); + EXPECT_EQ(ret_service_info[0]->response_count, 3); + EXPECT_EQ(ret_service_info[0]->serialization_format, "cdr"); +} + +void GetServiceInfoTest::topics_and_services_bag_test(std::string storage_id) +{ + const auto bag_path = get_bag_path(); + const std::string record_cmd = + "ros2 bag record --all-topics --all-services -s " + storage_id + " -o " + bag_path.string(); + std::string bag_filename; + + ASSERT_NO_THROW(bag_filename = get_bag_file_name(0, storage_id)); + + // Prepare service/client + auto service_client_manager1 = + std::make_shared>( + "test_service1"); + + if (!service_client_manager1->check_service_ready()) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + ASSERT_TRUE(service_client_manager1->check_service_ready()); + } + + auto service_client_manager2 = + std::make_shared>( + "test_service2"); + + if (!service_client_manager2->check_service_ready()) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + ASSERT_TRUE(service_client_manager2->check_service_ready()); + } + + rosbag2_test_common::PublicationManager pub_manager; + auto message = get_messages_strings()[0]; + pub_manager.setup_publisher("test_topic1", message, 1); + pub_manager.setup_publisher("test_topic2", message, 1); + + auto record_process = start_execution(record_cmd); + auto cleanup_process_handle = rcpputils::make_scope_exit( + [record_process]() { + stop_execution(record_process); + }); + + wait_for_storage_file(bag_path.string() + "/" + bag_filename); + + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + ASSERT_TRUE(service_client_manager1->send_request()); + ASSERT_TRUE(service_client_manager1->send_request()); + ASSERT_TRUE(service_client_manager2->send_request()); + pub_manager.run_publishers(); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + stop_execution(record_process); + cleanup_process_handle.cancel(); + + rosbag2_cpp::Info info; + std::vector> ret_service_info; + + ASSERT_NO_THROW( + ret_service_info = + info.read_service_info(bag_path.string() + "/" + bag_filename, storage_id)); + + ASSERT_EQ(ret_service_info.size(), 2); + if (ret_service_info[0]->name == "/test_service2") { + EXPECT_EQ(ret_service_info[0]->request_count, 1); + EXPECT_EQ(ret_service_info[0]->response_count, 1); + EXPECT_EQ(ret_service_info[1]->name, "/test_service1"); + EXPECT_EQ(ret_service_info[1]->request_count, 2); + EXPECT_EQ(ret_service_info[1]->response_count, 2); + } else { + EXPECT_EQ(ret_service_info[0]->name, "/test_service1"); + EXPECT_EQ(ret_service_info[0]->request_count, 2); + EXPECT_EQ(ret_service_info[0]->response_count, 2); + EXPECT_EQ(ret_service_info[1]->name, "/test_service2"); + EXPECT_EQ(ret_service_info[1]->request_count, 1); + EXPECT_EQ(ret_service_info[1]->response_count, 1); + } + EXPECT_EQ(ret_service_info[0]->type, "test_msgs/srv/BasicTypes"); + EXPECT_EQ(ret_service_info[0]->serialization_format, "cdr"); + EXPECT_EQ(ret_service_info[1]->type, "test_msgs/srv/BasicTypes"); + EXPECT_EQ(ret_service_info[1]->serialization_format, "cdr"); +} + +TEST_F(GetServiceInfoTest, topics_and_services_mcap_bag_test) { + topics_and_services_bag_test("mcap"); +} + +TEST_F(GetServiceInfoTest, topics_and_services_sqlite3_bag_test) { + topics_and_services_bag_test("sqlite3"); +} From 8a7637284dd6558fe50107f16d4ef7d8a6b5fc18 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 2 Nov 2023 15:12:00 +0800 Subject: [PATCH 11/35] Rename exclude_services to exclude_service_event Signed-off-by: Barry Xu --- ros2bag/ros2bag/verb/record.py | 2 +- rosbag2_py/src/rosbag2_py/_transport.cpp | 2 +- .../include/rosbag2_transport/record_options.hpp | 2 +- rosbag2_transport/src/rosbag2_transport/record_options.cpp | 4 ++-- rosbag2_transport/src/rosbag2_transport/topic_filter.cpp | 4 ++-- .../test/rosbag2_transport/test_record_options.cpp | 4 ++-- .../test/rosbag2_transport/test_record_regex.cpp | 2 +- .../test/rosbag2_transport/test_topic_filter.cpp | 4 ++-- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index a96c5a29d3..e2d5e484c4 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -189,7 +189,7 @@ def add_arguments(self, parser, cli_name): # noqa: D102 'Has no effect if no compression mode is chosen. Default: %(default)s.') def _check_necessary_argument(self, args): - # At least one options out of --all, --all-topics, --all-services, --services, topics or + # At least one options out of --all, --all-topics, --all-services, --services, --topics or # --regex must be used if not (args.all or args.all_topics or args.all_services or args.services or (args.topics and len(args.topics) > 0) or args.regex): diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index e36a1fb738..0e3cb12afb 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -358,7 +358,7 @@ PYBIND11_MODULE(_transport, m) { .def_readwrite("regex", &RecordOptions::regex) .def_readwrite("exclude_regex", &RecordOptions::exclude_regex) .def_readwrite("exclude_topics", &RecordOptions::exclude_topics) - .def_readwrite("exclude_services", &RecordOptions::exclude_services) + .def_readwrite("exclude_services", &RecordOptions::exclude_service_events) .def_readwrite("node_prefix", &RecordOptions::node_prefix) .def_readwrite("compression_mode", &RecordOptions::compression_mode) .def_readwrite("compression_format", &RecordOptions::compression_format) diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index 04c86a7761..840debaa32 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -36,7 +36,7 @@ struct RecordOptions std::vector topics; std::vector services; // service event topic std::vector exclude_topics; - std::vector exclude_services; // service event topic + std::vector exclude_service_events; // service event topic std::string rmw_serialization_format; std::chrono::milliseconds topic_polling_interval{100}; std::string regex = ""; diff --git a/rosbag2_transport/src/rosbag2_transport/record_options.cpp b/rosbag2_transport/src/rosbag2_transport/record_options.cpp index d72ef358a1..d9a7e4b1ae 100644 --- a/rosbag2_transport/src/rosbag2_transport/record_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/record_options.cpp @@ -37,7 +37,7 @@ Node convert::encode( node["regex"] = record_options.regex; node["exclude_regex"] = record_options.exclude_regex; node["exclude_topics"] = record_options.exclude_topics; - node["exclude_services"] = record_options.exclude_services; + node["exclude_services"] = record_options.exclude_service_events; node["node_prefix"] = record_options.node_prefix; node["compression_mode"] = record_options.compression_mode; node["compression_format"] = record_options.compression_format; @@ -69,7 +69,7 @@ bool convert::decode( optional_assign(node, "exclude_regex", record_options.exclude_regex); optional_assign>(node, "exclude_topics", record_options.exclude_topics); optional_assign>( - node, "exclude_services", record_options.exclude_services); + node, "exclude_services", record_options.exclude_service_events); 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/topic_filter.cpp b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp index 738d74024c..e0b82dbf73 100644 --- a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp +++ b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp @@ -220,8 +220,8 @@ bool TopicFilter::take_topic( } } - if (!record_options_.exclude_services.empty() && - service_in_list(topic_name, record_options_.exclude_services)) + if (!record_options_.exclude_service_events.empty() && + service_in_list(topic_name, record_options_.exclude_service_events)) { return false; } diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp index 6793c1ea11..015b1ae09e 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp @@ -27,7 +27,7 @@ TEST(record_options, test_yaml_serialization) original.topics = {"topic", "other_topic"}; original.services = {"service", "other_service"}; original.exclude_topics = {"exclude_topic1", "exclude_topic2"}; - original.exclude_services = {"exclude_service1", "exclude_service2"}; + original.exclude_service_events = {"exclude_service1", "exclude_service2"}; original.rmw_serialization_format = "cdr"; original.topic_polling_interval = std::chrono::milliseconds{200}; original.regex = "[xyz]/topic"; @@ -55,7 +55,7 @@ TEST(record_options, test_yaml_serialization) CHECK(topics); CHECK(services); CHECK(exclude_topics); - CHECK(exclude_services); + CHECK(exclude_service_events); CHECK(rmw_serialization_format); #undef CHECK } diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp index 0d02cc8805..1d154c32be 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp @@ -339,7 +339,7 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_service_service_recording rosbag2_transport::RecordOptions record_options = {false, false, false, {}, {}, {}, {}, "rmw_format", 10ms}; record_options.regex = regex; - record_options.exclude_services.emplace_back(services_exclude); + record_options.exclude_service_events.emplace_back(services_exclude); auto service_manager_v1 = std::make_shared>(v1); diff --git a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp index 30b9820853..7bb26b1676 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp @@ -236,7 +236,7 @@ TEST_F(RegexFixture, regex_all_services_and_exclude_regex) TEST_F(RegexFixture, regex_all_services_and_exclude_services) { rosbag2_transport::RecordOptions record_options; - record_options.exclude_services = { + record_options.exclude_service_events = { "/invalid_service/_service_event", "/invalidated_service/_service_event" }; @@ -296,7 +296,7 @@ TEST_F(RegexFixture, regex_filter_exclude_services) { rosbag2_transport::RecordOptions record_options; record_options.regex = "/invalid.*"; - record_options.exclude_services = {"/invalidated_service/_service_event"}; + record_options.exclude_service_events = {"/invalidated_service/_service_event"}; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_); From 8b21b63df18a04dfcbfc16ed185f94fa0bc30fd5 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 5 Nov 2023 00:38:02 -0700 Subject: [PATCH 12/35] Address flakiness in newly added rosbag2_transport tests - Got rid form ambient sleep for waiting for messages to be recorded - Exclude "/rosout" topic from recording Signed-off-by: Michael Orlov --- .../rosbag2_transport/test_record_all.cpp | 41 ++++++++++--------- .../rosbag2_transport/test_record_regex.cpp | 26 +++++++++--- 2 files changed, 42 insertions(+), 25 deletions(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp index 6680aa754b..2b818fd044 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp @@ -50,7 +50,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are pub_manager.setup_publisher(string_topic, string_message, 2); rosbag2_transport::RecordOptions record_options = - {true, false, false, {}, {}, {}, {}, "rmw_format", 100ms}; + {true, false, false, {}, {}, {"/rosout"}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -63,8 +63,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are pub_manager.run_publishers(); auto & writer = recorder->get_writer_handle(); - MockSequentialWriter & mock_writer = - static_cast(writer.get_implementation_handle()); + auto & mock_writer = dynamic_cast(writer.get_implementation_handle()); size_t expected_messages = 4; auto ret = rosbag2_test_common::wait_until_shutdown( @@ -72,11 +71,9 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are [&mock_writer, &expected_messages]() { return mock_writer.get_messages().size() >= expected_messages; }); - auto recorded_messages = mock_writer.get_messages(); - // We may receive additional messages from rosout, it doesn't matter, - // as long as we have received at least as many total messages as we expect EXPECT_TRUE(ret) << "failed to capture expected messages in time"; - EXPECT_THAT(recorded_messages, SizeIs(Ge(expected_messages))); + auto recorded_messages = mock_writer.get_messages(); + EXPECT_EQ(recorded_messages.size(), expected_messages); auto string_messages = filter_messages( recorded_messages, string_topic); @@ -100,7 +97,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_services_a "test_service_2"); rosbag2_transport::RecordOptions record_options = - {false, true, false, {}, {}, {}, {}, "rmw_format", 100ms}; + {false, true, false, {}, {}, {"/rosout"}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -111,18 +108,21 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_services_a ASSERT_TRUE(client_manager_2->check_service_ready()); - // By default, only client introspection is enable. + // By default, only client introspection is enabled. // 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()); + auto & mock_writer = dynamic_cast(writer.get_implementation_handle()); size_t expected_messages = 4; + auto ret = rosbag2_test_common::wait_until_shutdown( + std::chrono::seconds(5), + [&mock_writer, &expected_messages]() { + return mock_writer.get_messages().size() >= expected_messages; + }); + EXPECT_TRUE(ret) << "failed to capture expected messages in time"; auto recorded_messages = mock_writer.get_messages(); EXPECT_EQ(recorded_messages.size(), expected_messages); } @@ -140,7 +140,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_a pub_manager.setup_publisher(string_topic, string_message, 1); rosbag2_transport::RecordOptions record_options = - {true, true, false, {}, {}, {"rosout"}, {}, "rmw_format", 100ms}; + {true, true, false, {}, {}, {"/rosout"}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -153,17 +153,20 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_a pub_manager.run_publishers(); - // By default, only client introspection is enable. + // By default, only client introspection is enabled. // 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()); + auto & mock_writer = dynamic_cast(writer.get_implementation_handle()); size_t expected_messages = 3; + auto ret = rosbag2_test_common::wait_until_shutdown( + std::chrono::seconds(5), + [&mock_writer, &expected_messages]() { + return mock_writer.get_messages().size() >= expected_messages; + }); + EXPECT_TRUE(ret) << "failed to capture expected messages in time"; auto recorded_messages = mock_writer.get_messages(); EXPECT_EQ(recorded_messages.size(), expected_messages); } diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp index 1d154c32be..70511f7b34 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp @@ -303,16 +303,23 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_regex_service_recording) ASSERT_TRUE(service_manager_b2->check_service_ready()); auto & writer = recorder->get_writer_handle(); - MockSequentialWriter & mock_writer = - static_cast(writer.get_implementation_handle()); + auto & mock_writer = dynamic_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()); + + size_t expected_messages = 4; + auto ret = rosbag2_test_common::wait_until_shutdown( + std::chrono::seconds(5), + [&mock_writer, &expected_messages]() { + return mock_writer.get_messages().size() >= expected_messages; + }); + EXPECT_TRUE(ret) << "failed to capture expected messages in time"; auto recorded_messages = mock_writer.get_messages(); - EXPECT_THAT(recorded_messages, SizeIs(4)); + EXPECT_THAT(recorded_messages, SizeIs(expected_messages)); auto recorded_topics = mock_writer.get_topics(); EXPECT_THAT(recorded_topics, SizeIs(2)); @@ -369,16 +376,23 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_service_service_recording ASSERT_TRUE(service_manager_b2->check_service_ready()); auto & writer = recorder->get_writer_handle(); - MockSequentialWriter & mock_writer = - static_cast(writer.get_implementation_handle()); + auto & mock_writer = dynamic_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()); + + size_t expected_messages = 4; + auto ret = rosbag2_test_common::wait_until_shutdown( + std::chrono::seconds(5), + [&mock_writer, &expected_messages]() { + return mock_writer.get_messages().size() >= expected_messages; + }); + EXPECT_TRUE(ret) << "failed to capture expected messages in time"; auto recorded_messages = mock_writer.get_messages(); - EXPECT_THAT(recorded_messages, SizeIs(4)); + EXPECT_THAT(recorded_messages, SizeIs(expected_messages)); auto recorded_topics = mock_writer.get_topics(); EXPECT_THAT(recorded_topics, SizeIs(2)); From 3304efc1c1997c42c65bfb6823df13d72dd6f44b Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Mon, 6 Nov 2023 23:58:02 -0800 Subject: [PATCH 13/35] Use wait_for_srvice_to_be_ready() instead check_service_ready() - Also add default timeout parameter for ClientManager::send_request(..) Signed-off-by: Michael Orlov --- .../rosbag2_test_common/client_manager.hpp | 14 +++++++++++-- .../rosbag2_transport/test_record_all.cpp | 7 +++---- .../rosbag2_transport/test_record_regex.cpp | 20 +++++++++---------- 3 files changed, 25 insertions(+), 16 deletions(-) diff --git a/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp index a7764b4e49..958506758f 100644 --- a/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp +++ b/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp @@ -89,7 +89,17 @@ class ClientManager : public rclcpp::Node return true; } - bool send_request() + bool wait_for_srvice_to_be_ready(std::chrono::duration timeout = std::chrono::seconds(5)) + { + using clock = std::chrono::system_clock; + auto start = clock::now(); + while (!check_service_ready() && (clock::now() - start) < timeout) { + std::this_thread::sleep_for(std::chrono::milliseconds(25)); + } + return check_service_ready(); + } + + bool send_request(std::chrono::duration timeout = std::chrono::seconds(5)) { if (!check_service_ready()) { return false; @@ -100,7 +110,7 @@ class ClientManager : public rclcpp::Node 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) + exec_, get_node_base_interface(), result, timeout) != rclcpp::FutureReturnCode::SUCCESS) { RCLCPP_INFO( rclcpp::get_logger("service_client_manager"), "Failed to get response !"); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp index 2b818fd044..585ab0776a 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp @@ -104,9 +104,8 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_services_a start_async_spin(recorder); - ASSERT_TRUE(client_manager_1->check_service_ready()); - - ASSERT_TRUE(client_manager_2->check_service_ready()); + ASSERT_TRUE(client_manager_1->wait_for_srvice_to_be_ready()); + ASSERT_TRUE(client_manager_2->wait_for_srvice_to_be_ready()); // By default, only client introspection is enabled. // For one request, service event topic get 2 messages. @@ -149,7 +148,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_a ASSERT_TRUE(pub_manager.wait_for_matched(string_topic.c_str())); - ASSERT_TRUE(client_manager_1->check_service_ready()); + ASSERT_TRUE(client_manager_1->wait_for_srvice_to_be_ready()); pub_manager.run_publishers(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp index 70511f7b34..45e3a7bf08 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp @@ -296,11 +296,11 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_regex_service_recording) 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()); + ASSERT_TRUE(service_manager_v1->wait_for_srvice_to_be_ready()); + ASSERT_TRUE(service_manager_v2->wait_for_srvice_to_be_ready()); + ASSERT_TRUE(service_manager_e1->wait_for_srvice_to_be_ready()); + ASSERT_TRUE(service_manager_b1->wait_for_srvice_to_be_ready()); + ASSERT_TRUE(service_manager_b2->wait_for_srvice_to_be_ready()); auto & writer = recorder->get_writer_handle(); auto & mock_writer = dynamic_cast(writer.get_implementation_handle()); @@ -369,11 +369,11 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_service_service_recording 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()); + ASSERT_TRUE(service_manager_v1->wait_for_srvice_to_be_ready()); + ASSERT_TRUE(service_manager_v2->wait_for_srvice_to_be_ready()); + ASSERT_TRUE(service_manager_e1->wait_for_srvice_to_be_ready()); + ASSERT_TRUE(service_manager_b1->wait_for_srvice_to_be_ready()); + ASSERT_TRUE(service_manager_b2->wait_for_srvice_to_be_ready()); auto & writer = recorder->get_writer_handle(); auto & mock_writer = dynamic_cast(writer.get_implementation_handle()); From a8ba6c3efbd78288d5d000985c26d88f2a5f7fd7 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Tue, 7 Nov 2023 00:05:05 -0800 Subject: [PATCH 14/35] Add rosbag2_test_common::wait_until_condition(..) Signed-off-by: Michael Orlov --- .../include/rosbag2_test_common/wait_for.hpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/rosbag2_test_common/include/rosbag2_test_common/wait_for.hpp b/rosbag2_test_common/include/rosbag2_test_common/wait_for.hpp index d8406c286b..9578e72439 100644 --- a/rosbag2_test_common/include/rosbag2_test_common/wait_for.hpp +++ b/rosbag2_test_common/include/rosbag2_test_common/wait_for.hpp @@ -51,6 +51,23 @@ bool wait_until_shutdown(Timeout timeout, Condition condition) rclcpp::shutdown(); return true; } + +template +bool wait_until_condition( + Condition condition, + std::chrono::duration timeout = std::chrono::seconds(5)) +{ + using clock = std::chrono::system_clock; + auto start = clock::now(); + while (!condition()) { + if ((clock::now() - start) > timeout) { + return false; + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + return true; +} + } // namespace rosbag2_test_common #endif // ROSBAG2_TEST_COMMON__WAIT_FOR_HPP_ From a21ae1a936ea93b1fedaab81d7c4c38cd441a354 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Tue, 7 Nov 2023 00:09:10 -0800 Subject: [PATCH 15/35] Rewrite get_service_info tests to be more deterministic - Also parameterize tests to use default supported storage plugins Signed-off-by: Michael Orlov --- rosbag2_tests/CMakeLists.txt | 1 + rosbag2_tests/package.xml | 1 + .../test_rosbag2_info_get_service_info.cpp | 368 +++++++++++------- 3 files changed, 231 insertions(+), 139 deletions(-) diff --git a/rosbag2_tests/CMakeLists.txt b/rosbag2_tests/CMakeLists.txt index 664e6ab747..c9aecb9e44 100644 --- a/rosbag2_tests/CMakeLists.txt +++ b/rosbag2_tests/CMakeLists.txt @@ -129,6 +129,7 @@ if(BUILD_TESTING) rosbag2_cpp::rosbag2_cpp rosbag2_storage::rosbag2_storage rosbag2_test_common::rosbag2_test_common + rosbag2_transport::rosbag2_transport ${test_msgs_TARGETS} ) endif() diff --git a/rosbag2_tests/package.xml b/rosbag2_tests/package.xml index cbbfe413f2..8d9404100e 100644 --- a/rosbag2_tests/package.xml +++ b/rosbag2_tests/package.xml @@ -28,6 +28,7 @@ rosbag2_storage_default_plugins rosbag2_storage rosbag2_test_common + rosbag2_transport std_msgs test_msgs diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_get_service_info.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_get_service_info.cpp index 6cac3813a6..2e268ad6d5 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_get_service_info.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_get_service_info.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Sony Group Corporation. +// Copyright 2023 Sony Group Corporation and Apex.AI, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,226 +21,316 @@ #include "rosbag2_cpp/writer.hpp" #include "rosbag2_test_common/client_manager.hpp" -#include "rosbag2_test_common/process_execution_helpers.hpp" #include "rosbag2_test_common/publication_manager.hpp" #include "rosbag2_test_common/temporary_directory_fixture.hpp" +#include "rosbag2_test_common/tested_storage_ids.hpp" +#include "rosbag2_test_common/wait_for.hpp" + +#include "rosbag2_transport/recorder.hpp" #include "test_msgs/message_fixtures.hpp" #include "test_msgs/msg/basic_types.hpp" #include "test_msgs/srv/basic_types.hpp" -class GetServiceInfoTest : public rosbag2_test_common::TemporaryDirectoryFixture +class SequentialWriterForTest : public rosbag2_cpp::writers::SequentialWriter +{ +public: + size_t get_number_of_written_messages() + { + size_t num_messages = 0; + for (const auto & file : metadata_.files) { + num_messages += file.message_count; + } + return num_messages; + } +}; + +class GetServiceInfoTest : public rosbag2_test_common::ParametrizedTemporaryDirectoryFixture { public: void SetUp() override { - rclcpp::init(0, nullptr); + auto bag_name = get_test_name() + "_" + GetParam(); + root_bag_path_ = rcpputils::fs::path(temporary_dir_path_) / bag_name; + + // Clean up potentially leftover bag files. + // There may be leftovers if the system reallocates a temp directory + // used by a previous test execution and the test did not have a clean exit. + rcpputils::fs::remove_all(root_bag_path_); } void TearDown() override + { + rcpputils::fs::remove_all(root_bag_path_); + } + + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() { rclcpp::shutdown(); } - rcpputils::fs::path get_bag_path() + template + void start_async_spin(T node) { - return rcpputils::fs::path(temporary_dir_path_) / - UnitTest::GetInstance()->current_test_info()->name(); + node_spinner_future_ = std::async( + std::launch::async, + [&]() -> void { + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node); + while (!exit_from_node_spinner_) { + exec.spin_some(); + } + }); } - const std::string get_bag_file_name(int split_index, std::string storage_id) + void stop_spinning() { - if (storage_id == "mcap") { - return std::string(UnitTest::GetInstance()->current_test_info()->name()) + - "_" + std::to_string(split_index) + ".mcap"; - } else if (storage_id == "sqlite3") { - return std::string(UnitTest::GetInstance()->current_test_info()->name()) + - "_" + std::to_string(split_index) + ".db3"; - } else { - throw std::runtime_error("Invalid storage id : \"" + storage_id + "\""); + exit_from_node_spinner_ = true; + if (node_spinner_future_.valid()) { + node_spinner_future_.wait(); } } - void wait_for_storage_file( - std::string file_path, - std::chrono::duration timeout = std::chrono::seconds(10)) + std::string get_test_name() const + { + const auto * test_info = UnitTest::GetInstance()->current_test_info(); + std::string test_name = test_info->name(); + // Replace any slashes in the test name, since it is used in paths + std::replace(test_name.begin(), test_name.end(), '/', '_'); + return test_name; + } + + std::string get_bag_file_name(int split_index = 0) const + { + const auto storage_id = GetParam(); + std::stringstream bag_file_name; + bag_file_name << get_test_name() << "_" << storage_id << "_" << split_index; + return rosbag2_test_common::bag_filename_for_storage_id(bag_file_name.str(), storage_id); + } + + std::string get_bag_path_str() const + { + return root_bag_path_.string(); + } + + bool wait_for_subscriptions( + const rosbag2_transport::Recorder & recorder, + const std::vector && topic_names, + std::chrono::duration timeout = std::chrono::seconds(5)) { - auto check_file_path = rcpputils::fs::path(file_path); - const auto start_time = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start_time < timeout) { - if (check_file_path.exists()) { - return; + using clock = std::chrono::system_clock; + auto start = clock::now(); + bool ready = false; + while (!ready && (clock::now() - start) < timeout) { + const auto & subscriptions = recorder.subscriptions(); + ready = true; + for (const auto & topic_name : topic_names) { + if (subscriptions.find(topic_name) == subscriptions.end()) { + ready = false; + break; + } } - std::this_thread::sleep_for(std::chrono::milliseconds(50)); + std::this_thread::sleep_for(std::chrono::milliseconds(25)); } - ASSERT_EQ(check_file_path.exists(), true) - << "Could not find storage file: \"" << check_file_path.string() << "\""; + return ready; } - void topics_and_services_bag_test(std::string storage_id); + // relative path to the root of the bag file. + rcpputils::fs::path root_bag_path_; + std::future node_spinner_future_; + std::atomic_bool exit_from_node_spinner_{false}; }; -TEST_F(GetServiceInfoTest, only_topics_bag_test) { - const std::string storage_id = "mcap"; - const auto bag_path = get_bag_path(); - +TEST_P(GetServiceInfoTest, only_topics_bag_test) { + const std::string storage_id = GetParam(); + const auto bag_path_str = get_bag_path_str(); { // Create an empty bag with default storage rosbag2_cpp::Writer writer; rosbag2_storage::StorageOptions storage_options; storage_options.storage_id = storage_id; - storage_options.uri = bag_path.string(); + storage_options.uri = bag_path_str; writer.open(storage_options); test_msgs::msg::BasicTypes msg; writer.write(msg, "test_topic", rclcpp::Time{}); } - std::string first_storage_file_path; - { - rosbag2_storage::MetadataIo metadata_io; - auto metadata = metadata_io.read_metadata(bag_path.string()); - first_storage_file_path = (bag_path / metadata.relative_file_paths[0]).string(); - } rosbag2_cpp::Info info; - std::vector> ret_service_info; - - ASSERT_NO_THROW(ret_service_info = info.read_service_info(first_storage_file_path, storage_id)); + std::vector> ret_service_infos; + const std::string recorded_bag_uri = bag_path_str + "/" + get_bag_file_name(); + ASSERT_NO_THROW(ret_service_infos = info.read_service_info(recorded_bag_uri, storage_id)) << + recorded_bag_uri; - EXPECT_TRUE(ret_service_info.empty()); + EXPECT_TRUE(ret_service_infos.empty()); } -TEST_F(GetServiceInfoTest, only_services_bag_test) { - const std::string storage_id = "mcap"; - const auto bag_path = get_bag_path(); - const std::string record_cmd = "ros2 bag record --all-services -o " + bag_path.string(); - std::string bag_filename; - - ASSERT_NO_THROW(bag_filename = get_bag_file_name(0, storage_id)); +TEST_P(GetServiceInfoTest, only_services_bag_test) { + const std::string storage_id = GetParam(); + const std::string bag_path_str = get_bag_path_str(); auto service_client_manager = std::make_shared>( "test_service"); - if (!service_client_manager->check_service_ready()) { - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - ASSERT_TRUE(service_client_manager->check_service_ready()); - } + std::unique_ptr writer_impl = + std::make_unique(); + auto writer = std::make_unique(std::move(writer_impl)); + + rosbag2_storage::StorageOptions storage_options; + storage_options.storage_id = storage_id; + storage_options.uri = bag_path_str; + rosbag2_transport::RecordOptions record_options = + {false, true, false, {}, {}, {"/rosout"}, {}, "cdr", 100ms}; + auto recorder = std::make_shared( + std::move(writer), storage_options, record_options); + recorder->record(); - auto record_process = start_execution(record_cmd); + start_async_spin(recorder); auto cleanup_process_handle = rcpputils::make_scope_exit( - [record_process]() { - stop_execution(record_process); - }); + [&]() {stop_spinning();}); - wait_for_storage_file(bag_path.string() + "/" + bag_filename); + ASSERT_TRUE(service_client_manager->wait_for_srvice_to_be_ready()); + ASSERT_TRUE(wait_for_subscriptions(*recorder, {"/test_service/_service_event"})); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - ASSERT_TRUE(service_client_manager->send_request()); - ASSERT_TRUE(service_client_manager->send_request()); - ASSERT_TRUE(service_client_manager->send_request()); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); + constexpr size_t num_service_requests = 3; + for (size_t i = 0; i < num_service_requests; i++) { + ASSERT_TRUE(service_client_manager->send_request()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } - stop_execution(record_process); + auto & writer_ref = recorder->get_writer_handle(); + auto & recorder_writer = + dynamic_cast(writer_ref.get_implementation_handle()); + + // By default, only client introspection is enabled. + // For one request, service event topic get 2 messages. + size_t expected_messages = num_service_requests * 2; + auto ret = rosbag2_test_common::wait_until_condition( + [&recorder_writer, &expected_messages]() { + return recorder_writer.get_number_of_written_messages() >= expected_messages; + }, + std::chrono::seconds(5)); + EXPECT_TRUE(ret) << "Failed to capture " << expected_messages << " expected messages in time"; + + recorder->stop(); + stop_spinning(); cleanup_process_handle.cancel(); rosbag2_cpp::Info info; - std::vector> ret_service_info; - - ASSERT_NO_THROW( - ret_service_info = - info.read_service_info(bag_path.string() + "/" + bag_filename, storage_id)); - - ASSERT_EQ(ret_service_info.size(), 1); - EXPECT_EQ(ret_service_info[0]->name, "/test_service"); - EXPECT_EQ(ret_service_info[0]->type, "test_msgs/srv/BasicTypes"); - EXPECT_EQ(ret_service_info[0]->request_count, 3); - EXPECT_EQ(ret_service_info[0]->response_count, 3); - EXPECT_EQ(ret_service_info[0]->serialization_format, "cdr"); + std::vector> ret_service_infos; + + const std::string recorded_bag_uri = bag_path_str + "/" + get_bag_file_name(); + ASSERT_NO_THROW(ret_service_infos = info.read_service_info(recorded_bag_uri, storage_id)) << + recorded_bag_uri; + + ASSERT_EQ(ret_service_infos.size(), 1); + EXPECT_EQ(ret_service_infos[0]->name, "/test_service"); + EXPECT_EQ(ret_service_infos[0]->type, "test_msgs/srv/BasicTypes"); + EXPECT_EQ(ret_service_infos[0]->request_count, num_service_requests); + EXPECT_EQ(ret_service_infos[0]->response_count, num_service_requests); + EXPECT_EQ(ret_service_infos[0]->serialization_format, "cdr"); } -void GetServiceInfoTest::topics_and_services_bag_test(std::string storage_id) -{ - const auto bag_path = get_bag_path(); - const std::string record_cmd = - "ros2 bag record --all-topics --all-services -s " + storage_id + " -o " + bag_path.string(); - std::string bag_filename; - - ASSERT_NO_THROW(bag_filename = get_bag_file_name(0, storage_id)); +TEST_P(GetServiceInfoTest, topics_and_services_bag_test) { + const std::string storage_id = GetParam(); + const std::string bag_path_str = get_bag_path_str(); // Prepare service/client auto service_client_manager1 = std::make_shared>( "test_service1"); - - if (!service_client_manager1->check_service_ready()) { - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - ASSERT_TRUE(service_client_manager1->check_service_ready()); - } - auto service_client_manager2 = std::make_shared>( "test_service2"); - if (!service_client_manager2->check_service_ready()) { - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - ASSERT_TRUE(service_client_manager2->check_service_ready()); - } + ASSERT_TRUE(service_client_manager1->wait_for_srvice_to_be_ready()); + ASSERT_TRUE(service_client_manager2->wait_for_srvice_to_be_ready()); rosbag2_test_common::PublicationManager pub_manager; auto message = get_messages_strings()[0]; pub_manager.setup_publisher("test_topic1", message, 1); pub_manager.setup_publisher("test_topic2", message, 1); - auto record_process = start_execution(record_cmd); + std::unique_ptr writer_impl = + std::make_unique(); + auto writer = std::make_unique(std::move(writer_impl)); + rosbag2_storage::StorageOptions storage_options; + storage_options.storage_id = storage_id; + storage_options.uri = bag_path_str; + rosbag2_transport::RecordOptions record_options = + {true, true, false, {}, {}, {"/rosout"}, {}, "cdr", 100ms}; + auto recorder = std::make_shared( + std::move(writer), storage_options, record_options); + recorder->record(); + + start_async_spin(recorder); auto cleanup_process_handle = rcpputils::make_scope_exit( - [record_process]() { - stop_execution(record_process); - }); - - wait_for_storage_file(bag_path.string() + "/" + bag_filename); - - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - ASSERT_TRUE(service_client_manager1->send_request()); - ASSERT_TRUE(service_client_manager1->send_request()); - ASSERT_TRUE(service_client_manager2->send_request()); + [&]() {stop_spinning();}); + + ASSERT_TRUE( + wait_for_subscriptions( + *recorder, + {"/test_service1/_service_event", + "/test_service2/_service_event", + "/test_topic1", + "/test_topic2"} + ) + ); + + constexpr size_t num_service_requests = 2; + for (size_t i = 0; i < num_service_requests; i++) { + ASSERT_TRUE(service_client_manager1->send_request()); + ASSERT_TRUE(service_client_manager2->send_request()); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } pub_manager.run_publishers(); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - stop_execution(record_process); + auto & writer_ref = recorder->get_writer_handle(); + auto & recorder_writer = + dynamic_cast(writer_ref.get_implementation_handle()); + + // By default, only client introspection is enabled. + // For one request, service event topic get 2 messages. + size_t expected_messages = num_service_requests * 2 + 2; + auto ret = rosbag2_test_common::wait_until_condition( + [&recorder_writer, &expected_messages]() { + return recorder_writer.get_number_of_written_messages() >= expected_messages; + }, + std::chrono::seconds(5)); + EXPECT_TRUE(ret) << "Failed to capture " << expected_messages << " expected messages in time"; + + recorder->stop(); + stop_spinning(); cleanup_process_handle.cancel(); rosbag2_cpp::Info info; - std::vector> ret_service_info; - - ASSERT_NO_THROW( - ret_service_info = - info.read_service_info(bag_path.string() + "/" + bag_filename, storage_id)); - - ASSERT_EQ(ret_service_info.size(), 2); - if (ret_service_info[0]->name == "/test_service2") { - EXPECT_EQ(ret_service_info[0]->request_count, 1); - EXPECT_EQ(ret_service_info[0]->response_count, 1); - EXPECT_EQ(ret_service_info[1]->name, "/test_service1"); - EXPECT_EQ(ret_service_info[1]->request_count, 2); - EXPECT_EQ(ret_service_info[1]->response_count, 2); + std::vector> ret_service_infos; + + const std::string recorded_bag_uri = bag_path_str + "/" + get_bag_file_name(); + ASSERT_NO_THROW(ret_service_infos = info.read_service_info(recorded_bag_uri, storage_id)) << + recorded_bag_uri; + ASSERT_EQ(ret_service_infos.size(), 2); + if (ret_service_infos[0]->name == "/test_service2") { + EXPECT_EQ(ret_service_infos[1]->name, "/test_service1"); } else { - EXPECT_EQ(ret_service_info[0]->name, "/test_service1"); - EXPECT_EQ(ret_service_info[0]->request_count, 2); - EXPECT_EQ(ret_service_info[0]->response_count, 2); - EXPECT_EQ(ret_service_info[1]->name, "/test_service2"); - EXPECT_EQ(ret_service_info[1]->request_count, 1); - EXPECT_EQ(ret_service_info[1]->response_count, 1); - } - EXPECT_EQ(ret_service_info[0]->type, "test_msgs/srv/BasicTypes"); - EXPECT_EQ(ret_service_info[0]->serialization_format, "cdr"); - EXPECT_EQ(ret_service_info[1]->type, "test_msgs/srv/BasicTypes"); - EXPECT_EQ(ret_service_info[1]->serialization_format, "cdr"); -} - -TEST_F(GetServiceInfoTest, topics_and_services_mcap_bag_test) { - topics_and_services_bag_test("mcap"); + EXPECT_EQ(ret_service_infos[0]->name, "/test_service1"); + EXPECT_EQ(ret_service_infos[1]->name, "/test_service2"); + } + for (const auto & service_info : ret_service_infos) { + EXPECT_EQ(service_info->request_count, num_service_requests); + EXPECT_EQ(service_info->response_count, num_service_requests); + EXPECT_EQ(service_info->type, "test_msgs/srv/BasicTypes"); + EXPECT_EQ(service_info->serialization_format, "cdr"); + } } -TEST_F(GetServiceInfoTest, topics_and_services_sqlite3_bag_test) { - topics_and_services_bag_test("sqlite3"); -} +INSTANTIATE_TEST_SUITE_P( + TestInfoGetServiceInfo, + GetServiceInfoTest, + ::testing::ValuesIn(rosbag2_test_common::kTestedStorageIDs) +); From b242105450119d7a629ce94668373dfe86b77143 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Tue, 7 Nov 2023 00:21:31 -0800 Subject: [PATCH 16/35] Renames in the get_service_info tests Signed-off-by: Michael Orlov --- rosbag2_tests/CMakeLists.txt | 8 ++++---- ...info.cpp => test_rosbag2_cpp_get_service_info.cpp} | 11 ++++++----- 2 files changed, 10 insertions(+), 9 deletions(-) rename rosbag2_tests/test/rosbag2_tests/{test_rosbag2_info_get_service_info.cpp => test_rosbag2_cpp_get_service_info.cpp} (96%) diff --git a/rosbag2_tests/CMakeLists.txt b/rosbag2_tests/CMakeLists.txt index c9aecb9e44..d856d80dbc 100644 --- a/rosbag2_tests/CMakeLists.txt +++ b/rosbag2_tests/CMakeLists.txt @@ -121,11 +121,11 @@ if(BUILD_TESTING) ) endif() - ament_add_gmock(test_rosbag2_info_get_service_info - test/rosbag2_tests/test_rosbag2_info_get_service_info.cpp + ament_add_gmock(test_rosbag2_cpp_get_service_info + test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) - if(TARGET test_rosbag2_info_get_service_info) - target_link_libraries(test_rosbag2_info_get_service_info + if(TARGET test_rosbag2_cpp_get_service_info) + target_link_libraries(test_rosbag2_cpp_get_service_info rosbag2_cpp::rosbag2_cpp rosbag2_storage::rosbag2_storage rosbag2_test_common::rosbag2_test_common diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_get_service_info.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp similarity index 96% rename from rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_get_service_info.cpp rename to rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp index 2e268ad6d5..ee80f018c0 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_get_service_info.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp @@ -45,7 +45,8 @@ class SequentialWriterForTest : public rosbag2_cpp::writers::SequentialWriter } }; -class GetServiceInfoTest : public rosbag2_test_common::ParametrizedTemporaryDirectoryFixture +class Rosbag2CPPGetServiceInfoTest + : public rosbag2_test_common::ParametrizedTemporaryDirectoryFixture { public: void SetUp() override @@ -146,7 +147,7 @@ class GetServiceInfoTest : public rosbag2_test_common::ParametrizedTemporaryDire std::atomic_bool exit_from_node_spinner_{false}; }; -TEST_P(GetServiceInfoTest, only_topics_bag_test) { +TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_topics_only) { const std::string storage_id = GetParam(); const auto bag_path_str = get_bag_path_str(); { @@ -169,7 +170,7 @@ TEST_P(GetServiceInfoTest, only_topics_bag_test) { EXPECT_TRUE(ret_service_infos.empty()); } -TEST_P(GetServiceInfoTest, only_services_bag_test) { +TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_services_only) { const std::string storage_id = GetParam(); const std::string bag_path_str = get_bag_path_str(); @@ -236,7 +237,7 @@ TEST_P(GetServiceInfoTest, only_services_bag_test) { EXPECT_EQ(ret_service_infos[0]->serialization_format, "cdr"); } -TEST_P(GetServiceInfoTest, topics_and_services_bag_test) { +TEST_P(Rosbag2CPPGetServiceInfoTest, get_service_info_for_bag_with_topics_and_services) { const std::string storage_id = GetParam(); const std::string bag_path_str = get_bag_path_str(); @@ -331,6 +332,6 @@ TEST_P(GetServiceInfoTest, topics_and_services_bag_test) { INSTANTIATE_TEST_SUITE_P( TestInfoGetServiceInfo, - GetServiceInfoTest, + Rosbag2CPPGetServiceInfoTest, ::testing::ValuesIn(rosbag2_test_common::kTestedStorageIDs) ); From a21c2163e48d422d98a79835e33ee5df18991fba Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Tue, 7 Nov 2023 01:37:08 -0800 Subject: [PATCH 17/35] Use `std::filesystem` instead of `rcpputils::fs` functions - Rationale: Deprecation of the rcpputils::fs in future See https://github.com/ros2/rcpputils/issues/164 for details Signed-off-by: Michael Orlov --- .../test_rosbag2_cpp_get_service_info.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp index ee80f018c0..9fcefb8819 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include "rosbag2_cpp/info.hpp" @@ -52,17 +53,17 @@ class Rosbag2CPPGetServiceInfoTest void SetUp() override { auto bag_name = get_test_name() + "_" + GetParam(); - root_bag_path_ = rcpputils::fs::path(temporary_dir_path_) / bag_name; + root_bag_path_ = std::filesystem::path(temporary_dir_path_) / bag_name; // Clean up potentially leftover bag files. // There may be leftovers if the system reallocates a temp directory // used by a previous test execution and the test did not have a clean exit. - rcpputils::fs::remove_all(root_bag_path_); + std::filesystem::remove_all(root_bag_path_); } void TearDown() override { - rcpputils::fs::remove_all(root_bag_path_); + std::filesystem::remove_all(root_bag_path_); } static void SetUpTestCase() @@ -116,7 +117,7 @@ class Rosbag2CPPGetServiceInfoTest std::string get_bag_path_str() const { - return root_bag_path_.string(); + return root_bag_path_.generic_string(); } bool wait_for_subscriptions( @@ -142,7 +143,7 @@ class Rosbag2CPPGetServiceInfoTest } // relative path to the root of the bag file. - rcpputils::fs::path root_bag_path_; + std::filesystem::path root_bag_path_; std::future node_spinner_future_; std::atomic_bool exit_from_node_spinner_{false}; }; From 0fe42be4cdfe2d49c2d471bc300fde1168ed5e34 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Tue, 7 Nov 2023 00:23:05 -0800 Subject: [PATCH 18/35] Avoid extra metadata readout in `ros2 bag info` verb with `--verbose` Signed-off-by: Michael Orlov --- ros2bag/ros2bag/verb/info.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/ros2bag/ros2bag/verb/info.py b/ros2bag/ros2bag/verb/info.py index 5ab1000da0..d48bd06a9c 100644 --- a/ros2bag/ros2bag/verb/info.py +++ b/ros2bag/ros2bag/verb/info.py @@ -47,14 +47,14 @@ def _is_service_event_topic(self, topic_name, topic_type) -> bool: 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: - if not self._is_service_event_topic(topic_info.topic_metadata.name, - topic_info.topic_metadata.type): - print(topic_info.topic_metadata.name) + if args.verbose: + Info().read_metadata_and_output_service_verbose(args.bag_path, args.storage) else: - if args.verbose: - Info().read_metadata_and_output_service_verbose(args.bag_path, args.storage) + m = Info().read_metadata(args.bag_path, args.storage) + if args.topic_name: + for topic_info in m.topics_with_message_count: + if not self._is_service_event_topic(topic_info.topic_metadata.name, + topic_info.topic_metadata.type): + print(topic_info.topic_metadata.name) else: print(m) From d1725ed0277aaa6df96ea60da64ba20c34dac6c8 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Tue, 7 Nov 2023 01:14:25 -0800 Subject: [PATCH 19/35] Add info_end_to_end test with `--verbose` parameter Signed-off-by: Michael Orlov --- .../bag_with_topics_and_service_events.mcap | Bin 0 -> 13271 bytes .../metadata.yaml | 57 ++++++++++++++++++ .../bag_with_topics_and_service_events.db3 | Bin 0 -> 28672 bytes .../metadata.yaml | 57 ++++++++++++++++++ .../test_rosbag2_info_end_to_end.cpp | 50 +++++++++++++++ 5 files changed, 164 insertions(+) create mode 100644 rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/bag_with_topics_and_service_events.mcap create mode 100644 rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/metadata.yaml create mode 100644 rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/bag_with_topics_and_service_events.db3 create mode 100644 rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/metadata.yaml diff --git a/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/bag_with_topics_and_service_events.mcap b/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/bag_with_topics_and_service_events.mcap new file mode 100644 index 0000000000000000000000000000000000000000..e68cdbe4b722087550459da5436a44c22b54047e GIT binary patch literal 13271 zcmeHOO^h5z74F^H#hZy8=MNHt5-Mvc2Bh`$?{v=yi$FwTN5V#4D;zLtYPzd;+Vpmh zyL)%F=I@5^1#yYQfdc{r9N-27aWLY>4T(sRKqL|pNc>4~;fUb9`swZY*_~a+>sV&` z(AD+o)$5v9uU^0U>doy>y!g>i>Zh6uXEbpwL6K*Lc?!xT?rjEiK#cRodHv+2MZs}G zaP;Vg*#*)QP%T&<(c^Ri0ei?7Fk#@g`(}4MGkOloN>6oTvxu$NlJ;!!k z$J5>BCMybjV~}m7C5kJvK^zqRPF(h3l=Vh<^`}4646@BZ4s;ldA7%L_gRU|4<_7iFHI z7T@*R$Av|``tuvVU3ulTr!N2ITW?|MNxEp&G*xeWM4I5D_AIoOtSJ4>Vxwpk`F87L zw1|Uiy8~AEAKwNelV)lH!%67Y%d&*v&EKZU7Hjr)OGfY}>p`F91aGn)B^fO(lgP48 zZQFv{wk0)AOON19*0(r;-}g9-HnJG)8V`_=siMI%ypi8)TtGrD%iNO%BLGVEeryJk zC?pps8I}%AJrwz_wezhP>Ye`$wo`J^}Hzm1ljh0%RK0`xl07DSw-+(?( z!ebUiEC)w-6Kr{qXMDjv3vFwp50;(fyIo?zVhGs)moKgha27G`#YtR3n}KcM^M>k- zhDn@q9|xlSwCKMKq^p-MuIq;H^g12S zqqe6zRBuyenqd?fW=FTE>)JsOQrFN~$1&`V+3tB}JBk9&vzbLZwBwm<`70m`FZ_CG zn(`;A)Z}#k@L!kMU;p{kqYJ11_Qf}Dxx1Q%bNdk96iZbi?I@Rg1@E%_W!X~<=fcg; zardT5dPUF2JhV|I-*|`|bn|Bg!=c=sR`4ev+Je-;Ad}R6mdi%uhFpN-TT`loTKN(Sr zSpG7wP^?qe*C<1MjgBn;h5v}*sIsn>pFQ)_1BY{W)FP(m?^jm1T2!fiD_YCjH%7-H ztmRnLB8JobrWlSY>uS+|aqw_vQcGi3{;CD--5_W$iiAk9wa(+Eq82<}0q#k`)C8Iz zvWRXar9oCNvLwmKPL?O(bE~kE84Jb~uPJ%Y9fH>)=eXQe!F%o?xwhoJE(IUR?8re} zmy)XyuV1@*>GI3|i&rkMU%N2zM}7E$X+m7Rm4*-#iQhRCf$rO`6tIu7ruN%MaZ~&4 zqr|EG_EYH8j^oN*dR@AUMKJo4V1pw5O%ub<|Mzf{G3GdqWwoug*|BKg z1|2(So3>>#7WN$1by?4|JD%-^CNFR-bp)1GB6{{tNmT*8qw|M+e0SXGIh zOnna!MydM(>{Zd^(j2166*xqj!gtT!RoU&rxZRp_yA=r3KKr%15kB5EyA>9r;1*Rr zuy%XmJ-D3UTF@SayZXH^JjRJy)x5}?)iVC-BeMA=QNo_>1$a`y_5#KTaH(kb>$0xG ztJXuJUBx$dWc`E^x5UM%J?a^;SJc!d@dvW5g4m~5gg6yNeqYv4eiz*dTry-qSoSp_ z?E}l+f~J{ZSyyj6R>!uFfX*82PRFtx54^3b>#l7(_9!}QOo3wOB@*O8k8hO&#ovN-bP?soBh((SU0r1nNP7p<2!ueNo?*C* znTBpSo}-6h5IAi&jBL9T*+$1QJ8eDk_)EqRX$0vHw55Zfz}mRGWN+gz34WHW_uQKJ zbXF1G(Pr;~wx$^&#!1#>1MvvfGzz%Qt%*!@Y04Gd&%Ws$v?j4l+{buo7>4Ay=N@SO zJ)8VJn>Bjp^EvW8!_%b0cpz>wxcA3P1VwwFuer_Kw=vN1*vvfBL8OgL+L+)189oLb zN|or6Y6FCvqswth#=~j3spA{AISyTACMYa;94eK_`5cFa!+CMIa2Ijtc|Hwh0*U0A zc|Ohmu7}1SxI@FNhUekwaBxfQi0ymO=`6X{XC&H65`u|uB#TIi1jvVj2U?OHT0kw0 zi#`hpWYCcyfn>o@oq>rO&&O--BP(S)Bl3cr(GtX!8G)1)$Ti$z0M-Oq9)fF9lCuFz z;cN>>(wrmU?`)#btnpAafAf>KD(iL(ZRFd8k)1iv4u|a>oai<4{He^nc}CNNk-1l; zsLfL`ACL*XBafq(AsWC#s0oKN^P}QpKPq-+JUn^$RT?Q-vFa#I^41wni zfg6d$)a=FBfo`f~-_^H?fkNnj9yz`x>8<3%d6j5dK0y%ptV&Ls-a7b zDs*Mz+Fbp`k}2Czo^;#wjmdUs^}6uP?uLWqlKU zb7^zlwQ9NoMl~u|*UD)1TPrK`%|XLa2*iiu+yu@4hq&w9#P4DE;}R*|eDFLyk+ac1#yk(!9?@$$Pq6 zZ7^VdEts%?ylSZMc^*0*-6%a(LW^VfQg*e3G;BCzCNew6 zc+3)7Uw&mHosn}y%Sl=xlg{IuqG^(r!vzQ)K~geOCQ~3p$fPqlDVJ7NMakt0s+P?b zwQQy+3dMX{le|IN;w%lq97-VAaY@Rw38zcp5K6e}GLwgF=BLn;H_gdoPvmQ2`0PPtz zH2XLfFFnGA=Kmw3A9IsG82{tB+2lT)vW+3Y5MT%}1Q-Ggflm(tM_(FAa^mKTcbDta z9VqMmSZW}mqf|4jC_<(gPFsm)S{LP+J>7s9g!5x8vnOjKnL*azm0Hsgqbe)n=JDQJop00M% zl}(oyOUEzs!LF{;PS*=VN$#aW(l))IG`m!cJoD4uEOs+?HC54Z$FWxA(sl^H7lIJZ$0f{2TG>yREdhr3<5KnmY~N{H@)@{T9)_DzjUo~mT>5giawrG^a7VMK2q(E<_G5`|`8U;#)P z$QBkBc!onW_)bYS{J0bw;|kO7emNR^5q=?vyiPOGK}P@bAMY4$ z2F<)e48uZumTjnCm)@ zyis0XUcD|~TU*^IU+(DCS1te;Rfa6Q{~0kLPh zZh-b|cpTi|&UqI3KhJ-SOC3%=n!G#7PrN%ZHvW_G^XKM3$To%mLx3T`5MT%}1Q-Gg z0fs;;0`)CE!6g$%U!8jI)v@3G?X3s&SE0inhNkb9qB#t0+K>e)MT;!R99m>O^!{&S z)vL(_7Xwd+sH-1;@(4Y*uI`UfPwThho~LiT^&RSI9A@SIzm~~AxW}XRS-;dbGG z{^>BseV$f%XY@$7a8mqIe|xXZ{-{6lus`zojp)d0{G+y!lV>;bVZs~P@O^&$*Y`sw zsLziN+DC5n`APh3$LVmyH^6s&?ZTy~0pHgDbE4~KVZi=y1p9{{EkBN8U;XL#I~ouj zIC|dSoClGzANNQ0uz&dA@t>k2-(SaVBm3A#M~j{hH1gq41oVdw?x|7GfBBbNZ6o{f z7LFDk`yDaVi?%zA`3Cr|fB3;Kq6S>}dpqc@u17lxJMZThZO20cK6S{Y{+aq~>d&d) zrhb+BF!l4)&r0RC;PSvbEj+9&`^m1q=zF&YUAl)V{l6o62PHw*XFp>y#>k`DS1_=f)im$~C- literal 0 HcmV?d00001 diff --git a/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/metadata.yaml b/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/metadata.yaml new file mode 100644 index 0000000000..d0e796c5b1 --- /dev/null +++ b/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/metadata.yaml @@ -0,0 +1,57 @@ +rosbag2_bagfile_information: + version: 8 + storage_identifier: sqlite3 + duration: + nanoseconds: 70633730 + starting_time: + nanoseconds_since_epoch: 1699345836023194036 + message_count: 10 + topics_with_message_count: + - topic_metadata: + name: /events/write_split + type: rosbag2_interfaces/msg/WriteSplitEvent + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + type_description_hash: RIHS01_5ef58f7106a5cff8f5a794028c18117ee21015850ddcc567df449f41932960f8 + message_count: 0 + - topic_metadata: + name: /test_service1/_service_event + type: test_msgs/srv/BasicTypes_Event + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + type_description_hash: RIHS01_5b9988a4805a06ae22dff12903a774ccda710e9514926b826ffc884e3a9a982e + message_count: 4 + - topic_metadata: + name: /test_service2/_service_event + type: test_msgs/srv/BasicTypes_Event + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + type_description_hash: RIHS01_5b9988a4805a06ae22dff12903a774ccda710e9514926b826ffc884e3a9a982e + message_count: 4 + - topic_metadata: + name: /test_topic1 + type: test_msgs/msg/Strings + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + type_description_hash: RIHS01_eee55533636293ac7c94c62432eedb5777eb849847d2a17b4dcead8c4a20ab32 + message_count: 1 + - topic_metadata: + name: /test_topic2 + type: test_msgs/msg/Strings + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 9223372036\n nsec: 854775807\n lifespan:\n sec: 9223372036\n nsec: 854775807\n liveliness: 1\n liveliness_lease_duration:\n sec: 9223372036\n nsec: 854775807\n avoid_ros_namespace_conventions: false" + type_description_hash: RIHS01_eee55533636293ac7c94c62432eedb5777eb849847d2a17b4dcead8c4a20ab32 + message_count: 1 + compression_format: "" + compression_mode: "" + relative_file_paths: + - bag_with_topics_and_service_events.db3 + files: + - path: bag_with_topics_and_service_events.db3 + starting_time: + nanoseconds_since_epoch: 1699345836023194036 + duration: + nanoseconds: 70633730 + message_count: 10 + custom_data: ~ + ros_distro: "" \ No newline at end of file diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp index de1787e389..ae4f9a7c37 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp @@ -68,6 +68,56 @@ TEST_P(InfoEndToEndTestFixture, info_end_to_end_test) { "Serialization Format: cdr")); } +TEST_P(InfoEndToEndTestFixture, info_with_verbose_option_end_to_end_test) { + internal::CaptureStdout(); + auto exit_code = execute_and_wait_until_completion( + "ros2 bag info bag_with_topics_and_service_events --verbose", + bags_path_); + std::string output = internal::GetCapturedStdout(); + auto expected_storage = GetParam(); + auto expected_file = rosbag2_test_common::bag_filename_for_storage_id( + "bag_with_topics_and_service_events", GetParam()); + std::string expected_ros_distro = "unknown"; + + EXPECT_THAT(exit_code, Eq(EXIT_SUCCESS)); + // The bag size depends on the os and is not asserted, the time is asserted time zone independent + EXPECT_THAT( + output, ContainsRegex( + "\nFiles: " + expected_file + + "\nBag size: .*B" + "\nStorage id: " + expected_storage + + "\nROS Distro: " + expected_ros_distro + + "\nDuration: 0\\.70s" + "\nStart: Nov 7 2023 00:30:36\\..* \\(1699345836\\..*\\)" + "\nEnd: Nov 7 2023 00:30:36\\..* \\(1699345836\\..*\\)" + "\nMessages: 2" + "\nTopic information: ")); + EXPECT_THAT( + output, HasSubstr( + "Topic: /events/write_split | Type: rosbag2_interfaces/msg/WriteSplitEvent | Count: 0 | " + "Serialization Format: cdr\n")); + EXPECT_THAT( + output, HasSubstr( + "Topic: /test_topic1 | Type: test_msgs/msg/Strings | Count: 1 | " + "Serialization Format: cdr\n")); + EXPECT_THAT( + output, HasSubstr( + "Topic: /test_topic2 | Type: test_msgs/msg/Strings | Count: 1 | " + "Serialization Format: cdr\n")); + + EXPECT_THAT(output, HasSubstr("Service: 2\n")); + + EXPECT_THAT( + output, HasSubstr( + "Service: /test_service1 | Type: test_msgs/srv/BasicTypes | Request Count: 2 | " + "Response Count: 2 | Serialization Format: cdr\n")); + + EXPECT_THAT( + output, HasSubstr( + "Service: /test_service2 | Type: test_msgs/srv/BasicTypes | Request Count: 2 | " + "Response Count: 2 | Serialization Format: cdr\n")); +} + // TODO(Martin-Idel-SI): Revisit exit code non-zero here, gracefully should be exit code zero TEST_P(InfoEndToEndTestFixture, info_fails_gracefully_if_bag_does_not_exist) { internal::CaptureStderr(); From 75a3647b9e85a1e6179c71ab23522b1b00f69ad2 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 8 Nov 2023 14:55:33 +0800 Subject: [PATCH 20/35] Fix bugs in info_with_verbose_option_end_to_end_test Signed-off-by: Barry Xu --- .../test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp index ae4f9a7c37..0a8b16da92 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp @@ -88,8 +88,8 @@ TEST_P(InfoEndToEndTestFixture, info_with_verbose_option_end_to_end_test) { "\nStorage id: " + expected_storage + "\nROS Distro: " + expected_ros_distro + "\nDuration: 0\\.70s" - "\nStart: Nov 7 2023 00:30:36\\..* \\(1699345836\\..*\\)" - "\nEnd: Nov 7 2023 00:30:36\\..* \\(1699345836\\..*\\)" + "\nStart: Nov 7 2023 16:30:36\\..* \\(1699345836\\..*\\)" + "\nEnd: Nov 7 2023 16:30:36\\..* \\(1699345836\\..*\\)" "\nMessages: 2" "\nTopic information: ")); EXPECT_THAT( From 893d11d0f1df018bab059e7767685a55883d5a61 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 8 Nov 2023 16:01:53 +0800 Subject: [PATCH 21/35] Fix segment fault issue in test_rosbag2_cpp_get_service_info.cpp Signed-off-by: Barry Xu --- .../test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp index 9fcefb8819..0713095156 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp @@ -81,7 +81,7 @@ class Rosbag2CPPGetServiceInfoTest { node_spinner_future_ = std::async( std::launch::async, - [&]() -> void { + [node, this]() -> void { rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(node); while (!exit_from_node_spinner_) { From 0d8707f8f7ae7d32e7f2e15aaaa7c84930cea030 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 9 Nov 2023 09:24:24 +0800 Subject: [PATCH 22/35] Update a fix for info_with_verbose_option_end_to_end_test Signed-off-by: Barry Xu --- .../test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp index 0a8b16da92..4a87125ffc 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp @@ -88,8 +88,8 @@ TEST_P(InfoEndToEndTestFixture, info_with_verbose_option_end_to_end_test) { "\nStorage id: " + expected_storage + "\nROS Distro: " + expected_ros_distro + "\nDuration: 0\\.70s" - "\nStart: Nov 7 2023 16:30:36\\..* \\(1699345836\\..*\\)" - "\nEnd: Nov 7 2023 16:30:36\\..* \\(1699345836\\..*\\)" + "\nStart: Nov 7 2023 .*:30:36\\..* \\(1699345836\\..*\\)" + "\nEnd: Nov 7 2023 .*:30:36\\..* \\(1699345836\\..*\\)" "\nMessages: 2" "\nTopic information: ")); EXPECT_THAT( From 8237b50e003e62fe10c6bbec16830951c957a93c Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 9 Nov 2023 09:44:16 +0800 Subject: [PATCH 23/35] Output a warning while setting both '-t' and 'v' for info command Signed-off-by: Barry Xu --- ros2bag/ros2bag/verb/info.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ros2bag/ros2bag/verb/info.py b/ros2bag/ros2bag/verb/info.py index d48bd06a9c..160f128c8c 100644 --- a/ros2bag/ros2bag/verb/info.py +++ b/ros2bag/ros2bag/verb/info.py @@ -47,6 +47,9 @@ def _is_service_event_topic(self, topic_name, topic_type) -> bool: return True def main(self, *, args): # noqa: D102 + if args.topic_name and args.verbose: + print("Warning! You have set both the '-t' and '-v' parameters. The '-t' parameter " + 'will be ignored.') if args.verbose: Info().read_metadata_and_output_service_verbose(args.bag_path, args.storage) else: From 4b73ba6b91f7ee29296fbd1ba7d59939a687cfa8 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 5 Dec 2023 15:32:57 +0800 Subject: [PATCH 24/35] Address comments from Fujita-san Signed-off-by: Barry Xu --- docs/message_definition_encoding.md | 1 + .../include/rosbag2_cpp/service_utils.hpp | 2 ++ rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 14 +++++--- .../rosbag2_test_common/client_manager.hpp | 4 +-- .../test_rosbag2_info_end_to_end.cpp | 34 +++++++++++++++++++ .../src/rosbag2_transport/topic_filter.cpp | 1 + 6 files changed, 49 insertions(+), 7 deletions(-) diff --git a/docs/message_definition_encoding.md b/docs/message_definition_encoding.md index 7d6f5a7554..a3f5d950d5 100644 --- a/docs/message_definition_encoding.md +++ b/docs/message_definition_encoding.md @@ -39,6 +39,7 @@ 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 diff --git a/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp index 188d18396c..340f014319 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp @@ -25,10 +25,12 @@ ROSBAG2_CPP_PUBLIC bool is_service_event_topic(const std::string & topic, const std::string & topic_type); +// Call this function after is_service_event_topic() return true ROSBAG2_CPP_PUBLIC std::string service_event_topic_name_to_service_name(const std::string & topic_name); +// Call this function after is_service_event_topic() return true ROSBAG2_CPP_PUBLIC std::string service_event_topic_type_to_service_type(const std::string & topic_type); diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp index a456cb62ce..b6a4447108 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -38,6 +38,11 @@ bool is_service_event_topic(const std::string & topic, const std::string & topic std::string end_topic_name = topic.substr( topic.length() - (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)); + // Should be "/_service_event" + if (end_topic_name != RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) { + return false; + } + if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { return false; } @@ -51,11 +56,10 @@ bool is_service_event_topic(const std::string & topic, const std::string & topic 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); + 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; } std::string service_event_topic_name_to_service_name(const std::string & topic_name) diff --git a/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp index 958506758f..7189489578 100644 --- a/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp +++ b/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp @@ -43,7 +43,7 @@ class ClientManager : public rclcpp::Node enable_service_event_contents_(service_event_contents), enable_client_event_contents_(client_event_contents) { - auto echo_process = + auto do_nothing_srv_callback = [this](const std::shared_ptr request_header, const std::shared_ptr request, std::shared_ptr response) -> void @@ -54,7 +54,7 @@ class ClientManager : public rclcpp::Node (void)response; }; - service_ = create_service(service_name_, echo_process); + service_ = create_service(service_name_, do_nothing_srv_callback); rcl_service_introspection_state_t introspection_state; if (enable_service_event_contents_) { diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp index 4a87125ffc..cd5f0a41c0 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp @@ -68,6 +68,40 @@ TEST_P(InfoEndToEndTestFixture, info_end_to_end_test) { "Serialization Format: cdr")); } +TEST_P(InfoEndToEndTestFixture, info_with_verbose_option_and_topic_name_option) { + internal::CaptureStdout(); + auto exit_code = execute_and_wait_until_completion( + "ros2 bag info bag_with_topics_and_service_events --verbose --topic-name", + bags_path_); + std::string output = internal::GetCapturedStdout(); + auto expected_storage = GetParam(); + auto expected_file = rosbag2_test_common::bag_filename_for_storage_id( + "bag_with_topics_and_service_events", GetParam()); + std::string expected_ros_distro = "unknown"; + + EXPECT_THAT(exit_code, Eq(EXIT_SUCCESS)); + + EXPECT_THAT( + output, HasSubstr( + "Warning! You have set both the '-t' and '-v' parameters. The '-t' parameter " + "will be ignored.\n")); + + // The bag size depends on the os and is not asserted, the time is asserted time zone independent + EXPECT_THAT( + output, ContainsRegex( + "\nFiles: " + expected_file + + "\nBag size: .*B" + "\nStorage id: " + expected_storage + + "\nROS Distro: " + expected_ros_distro + + "\nDuration: 0\\.70s" + "\nStart: Nov 7 2023 .*:30:36\\..* \\(1699345836\\..*\\)" + "\nEnd: Nov 7 2023 .*:30:36\\..* \\(1699345836\\..*\\)" + "\nMessages: 2" + "\nTopic information: ")); + + EXPECT_THAT(output, HasSubstr("Service: 2\n")); +} + TEST_P(InfoEndToEndTestFixture, info_with_verbose_option_end_to_end_test) { internal::CaptureStdout(); auto exit_code = execute_and_wait_until_completion( diff --git a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp index e0b82dbf73..b42a9f8beb 100644 --- a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp +++ b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp @@ -71,6 +71,7 @@ bool topic_in_list(const std::string & topic_name, const std::vector & service_event_topics) From 116f5c3ed96f44186a893addac9ab92824068e9e Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 9 Dec 2023 22:41:21 -0800 Subject: [PATCH 25/35] Use topic_in_list instead of service_in_list Signed-off-by: Michael Orlov --- .../src/rosbag2_transport/topic_filter.cpp | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp index b42a9f8beb..cfb097ad96 100644 --- a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp +++ b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp @@ -71,19 +71,6 @@ 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) @@ -207,7 +194,7 @@ bool TopicFilter::take_topic( if (!record_options_.all_services) { // Not in include service list if (record_options_.services.empty() || - !service_in_list(topic_name, record_options_.services)) + !topic_in_list(topic_name, record_options_.services)) { // Not match include regex if (!record_options_.regex.empty()) { @@ -222,7 +209,7 @@ bool TopicFilter::take_topic( } if (!record_options_.exclude_service_events.empty() && - service_in_list(topic_name, record_options_.exclude_service_events)) + topic_in_list(topic_name, record_options_.exclude_service_events)) { return false; } From 186b035bffe6115cc6903fa4c765894380ee2ae0 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sat, 9 Dec 2023 22:43:51 -0800 Subject: [PATCH 26/35] Handle cases when dependent service msg definitions could be in IDL file Signed-off-by: Michael Orlov --- .../local_message_definition_source.cpp | 81 +++++++++++++++---- .../test_local_message_definition_source.cpp | 36 ++++++++- rosbag2_test_msgdefs/CMakeLists.txt | 3 +- rosbag2_test_msgdefs/srv/ComplexSrvIdl.srv | 3 + .../srv/{ComplexSrv.srv => ComplexSrvMsg.srv} | 0 5 files changed, 101 insertions(+), 22 deletions(-) create mode 100644 rosbag2_test_msgdefs/srv/ComplexSrvIdl.srv rename rosbag2_test_msgdefs/srv/{ComplexSrv.srv => ComplexSrvMsg.srv} (100%) 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 1d8e5c5db5..15b68ce106 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 @@ -102,10 +102,18 @@ 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); + case LocalMessageDefinitionSource::Format::SRV: + { + auto dep = parse_msg_dependencies(text, package_context); + if (!dep.empty()) { + return dep; + } else { + return parse_idl_dependencies(text); + } + } default: throw std::runtime_error("switch is not exhaustive"); } @@ -204,9 +212,7 @@ rosbag2_storage::MessageDefinition LocalMessageDefinitionSource::get_full_text( const MessageSpec & spec = load_message_spec(definition_identifier); std::string result = spec.text; for (const auto & dep_name : spec.dependencies) { - Format format = definition_identifier.format() == Format::SRV ? - Format::MSG : definition_identifier.format(); - DefinitionIdentifier dep(dep_name, format); + DefinitionIdentifier dep(dep_name, definition_identifier.format()); bool inserted = seen_deps.insert(dep).second; if (inserted) { result += "\n"; @@ -218,21 +224,62 @@ rosbag2_storage::MessageDefinition LocalMessageDefinitionSource::get_full_text( }; std::string result; - Format format = root_type.find("/srv/") != std::string::npos ? Format::SRV : Format::MSG; + Format format = Format::UNKNOWN; int32_t max_recursion_depth = ROSBAG2_CPP_LOCAL_MESSAGE_DEFINITION_SOURCE_MAX_RECURSION_DEPTH; - try { - 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_type, format); - result = (delimiter(root_definition_identifier) + - append_recursive(root_definition_identifier, max_recursion_depth)); - } catch (const TypenameNotUnderstoodError & err) { - ROSBAG2_CPP_LOG_ERROR( - "Message type name '%s' not understood by type definition search, " - "definition will be left empty in bag.", err.what()); + + if (root_type.find("/srv/") == std::string::npos) { // Not a service + try { + format = Format::MSG; + 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_type, format); + result = (delimiter(root_definition_identifier) + + append_recursive(root_definition_identifier, max_recursion_depth)); + } catch (const TypenameNotUnderstoodError & err) { + ROSBAG2_CPP_LOG_ERROR( + "Message type name '%s' not understood by type definition search, " + "definition will be left empty in bag.", err.what()); + format = Format::UNKNOWN; + } + } else { + // The service dependencies could be either in the msg or idl files. Therefore, will try to + // search service dependencies in MSG files first then in IDL files via two separate recursive + // searches for each dependency. format = Format::UNKNOWN; + DefinitionIdentifier def_identifier{root_type, Format::SRV}; + (void)seen_deps.insert(def_identifier).second; + result = delimiter(def_identifier); + const MessageSpec & spec = load_message_spec(def_identifier); + result += spec.text; + for (const auto & dep_name : spec.dependencies) { + DefinitionIdentifier dep(dep_name, Format::MSG); + bool inserted = seen_deps.insert(dep).second; + if (inserted) { + try { + result += "\n"; + result += delimiter(dep); + result += append_recursive(dep, max_recursion_depth); + format = Format::MSG; + } catch (const DefinitionNotFoundError & err) { + ROSBAG2_CPP_LOG_WARN("No .msg definition for %s, falling back to IDL", err.what()); + dep = DefinitionIdentifier(dep_name, Format::IDL); + inserted = seen_deps.insert(dep).second; + if (inserted) { + result += "\n"; + result += delimiter(dep); + result += append_recursive(dep, max_recursion_depth); + format = Format::IDL; + } + } catch (const TypenameNotUnderstoodError & err) { + ROSBAG2_CPP_LOG_ERROR( + "Message type name '%s' not understood by type definition search, " + "definition will be left empty in bag.", err.what()); + format = Format::UNKNOWN; + } + } + } } rosbag2_storage::MessageDefinition out; switch (format) { 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 526847dee6..e9dcc658df 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,21 +62,49 @@ TEST(test_local_message_definition_source, can_find_msg_deps) "float32 c\n"); } -TEST(test_local_message_definition_source, can_find_srv_deps) +TEST(test_local_message_definition_source, can_find_srv_deps_in_msg) { LocalMessageDefinitionSource source; - auto result = source.get_full_text("rosbag2_test_msgdefs/srv/ComplexSrv"); - std::cout << result.encoded_message_definition << std::endl; + auto result = source.get_full_text("rosbag2_test_msgdefs/srv/ComplexSrvMsg"); ASSERT_EQ(result.encoding, "ros2msg"); ASSERT_EQ( result.encoded_message_definition, + "================================================================================\n" + "SRV: rosbag2_test_msgdefs/srv/ComplexSrvMsg\n" "rosbag2_test_msgdefs/BasicMsg req\n" "---\n" "rosbag2_test_msgdefs/BasicMsg resp\n" "\n" "================================================================================\n" "MSG: rosbag2_test_msgdefs/BasicMsg\n" - "float32 c\n"); + "float32 c\n") << result.encoded_message_definition << std::endl; +} + +TEST(test_local_message_definition_source, can_find_srv_deps_in_idl) +{ + LocalMessageDefinitionSource source; + auto result = source.get_full_text("rosbag2_test_msgdefs/srv/ComplexSrvIdl"); + ASSERT_EQ(result.encoding, "ros2idl"); + ASSERT_EQ( + result.encoded_message_definition, + "================================================================================\n" + "SRV: rosbag2_test_msgdefs/srv/ComplexSrvIdl\n" + "rosbag2_test_msgdefs/BasicIdl req\n" + "---\n" + "rosbag2_test_msgdefs/BasicIdl resp\n" + "\n" + "================================================================================\n" + "MSG: rosbag2_test_msgdefs/BasicIdl\n" + "\n" + "================================================================================\n" + "IDL: rosbag2_test_msgdefs/BasicIdl\n" + "module rosbag2_test_msgdefs {\n" + " module msg {\n" + " struct BasicIdl {\n" + " float x;\n" + " };\n" + " };\n" + "};\n") << result.encoded_message_definition << std::endl; } TEST(test_local_message_definition_source, can_find_idl_deps) diff --git a/rosbag2_test_msgdefs/CMakeLists.txt b/rosbag2_test_msgdefs/CMakeLists.txt index f50036ab08..adacc5feab 100644 --- a/rosbag2_test_msgdefs/CMakeLists.txt +++ b/rosbag2_test_msgdefs/CMakeLists.txt @@ -15,7 +15,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ComplexMsg.msg" "msg/ComplexMsgDependsOnIdl.msg" "srv/BasicSrv.srv" - "srv/ComplexSrv.srv" + "srv/ComplexSrvMsg.srv" + "srv/ComplexSrvIdl.srv" ADD_LINTER_TESTS ) diff --git a/rosbag2_test_msgdefs/srv/ComplexSrvIdl.srv b/rosbag2_test_msgdefs/srv/ComplexSrvIdl.srv new file mode 100644 index 0000000000..6d467c6788 --- /dev/null +++ b/rosbag2_test_msgdefs/srv/ComplexSrvIdl.srv @@ -0,0 +1,3 @@ +rosbag2_test_msgdefs/BasicIdl req +--- +rosbag2_test_msgdefs/BasicIdl resp diff --git a/rosbag2_test_msgdefs/srv/ComplexSrv.srv b/rosbag2_test_msgdefs/srv/ComplexSrvMsg.srv similarity index 100% rename from rosbag2_test_msgdefs/srv/ComplexSrv.srv rename to rosbag2_test_msgdefs/srv/ComplexSrvMsg.srv From 999954341ce7fd1580593e4fe7fd698834cca129 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Sun, 10 Dec 2023 00:21:24 -0800 Subject: [PATCH 27/35] Fix rebase errors - Delete "rosbag2_transport/qos.cpp" which was moved to the rosbag2_storage package during prior PR before rebase Signed-off-by: Michael Orlov --- .../src/rosbag2_transport/qos.cpp | 234 ------------------ 1 file changed, 234 deletions(-) delete mode 100644 rosbag2_transport/src/rosbag2_transport/qos.cpp diff --git a/rosbag2_transport/src/rosbag2_transport/qos.cpp b/rosbag2_transport/src/rosbag2_transport/qos.cpp deleted file mode 100644 index 4233a1862f..0000000000 --- a/rosbag2_transport/src/rosbag2_transport/qos.cpp +++ /dev/null @@ -1,234 +0,0 @@ -// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. -// -// 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 "rosbag2_cpp/service_utils.hpp" -#include "rosbag2_transport/qos.hpp" -#include "logging.hpp" - -namespace -{ -/** - * The following constants were the "Inifinity" value returned by RMW implementations before - * the introduction of RMW_DURATION_INFINITE and associated RMW fixes - * RMW: https://github.com/ros2/rmw/pull/301 - * Fast-DDS: https://github.com/ros2/rmw_fastrtps/pull/515 - * CycloneDDS: https://github.com/ros2/rmw_cyclonedds/pull/288 - * RTI Connext: https://github.com/ros2/rmw_connext/pull/491 - * - * These values exist in bags recorded in Foxy, they need to be translated to RMW_DURATION_INFINITE - * to be consistently understood for playback. - * With those values, if a bag is played back in a different implementation than it was recorded, - * the publishers will fail to be created with an error indicating an invalid QoS value.. - */ -static const rmw_time_t RMW_CYCLONEDDS_FOXY_INFINITE = rmw_time_from_nsec(0x7FFFFFFFFFFFFFFFll); -static const rmw_time_t RMW_FASTRTPS_FOXY_INFINITE {0x7FFFFFFFll, 0xFFFFFFFFll}; -static const rmw_time_t RMW_CONNEXT_FOXY_INFINITE {0x7FFFFFFFll, 0x7FFFFFFFll}; -} // namespace - -namespace YAML -{ -Node convert::encode(const rmw_time_t & time) -{ - Node node; - node["sec"] = time.sec; - node["nsec"] = time.nsec; - return node; -} - -bool convert::decode(const Node & node, rmw_time_t & time) -{ - time.sec = node["sec"].as(); - time.nsec = node["nsec"].as(); - if ( - rmw_time_equal(time, RMW_CYCLONEDDS_FOXY_INFINITE) || - rmw_time_equal(time, RMW_FASTRTPS_FOXY_INFINITE) || - rmw_time_equal(time, RMW_CONNEXT_FOXY_INFINITE)) - { - time = RMW_DURATION_INFINITE; - } - return true; -} - -Node convert::encode(const rosbag2_transport::Rosbag2QoS & qos) -{ - const auto & p = qos.get_rmw_qos_profile(); - Node node; - node["history"] = static_cast(p.history); - node["depth"] = p.depth; - node["reliability"] = static_cast(p.reliability); - node["durability"] = static_cast(p.durability); - node["deadline"] = p.deadline; - node["lifespan"] = p.lifespan; - node["liveliness"] = static_cast(p.liveliness); - node["liveliness_lease_duration"] = p.liveliness_lease_duration; - node["avoid_ros_namespace_conventions"] = p.avoid_ros_namespace_conventions; - return node; -} - -bool convert::decode( - const Node & node, rosbag2_transport::Rosbag2QoS & qos) -{ - auto history = static_cast(node["history"].as()); - auto reliability = static_cast(node["reliability"].as()); - auto durability = static_cast(node["durability"].as()); - auto liveliness = static_cast(node["liveliness"].as()); - - qos - .keep_last(node["depth"].as()) - .history(history) - .reliability(reliability) - .durability(durability) - .deadline(node["deadline"].as()) - .lifespan(node["lifespan"].as()) - .liveliness(liveliness) - .liveliness_lease_duration(node["liveliness_lease_duration"].as()) - .avoid_ros_namespace_conventions(node["avoid_ros_namespace_conventions"].as()); - return true; -} -} // namespace YAML - -namespace rosbag2_transport -{ -Rosbag2QoS Rosbag2QoS::adapt_request_to_offers( - const std::string & topic_name, const std::vector & endpoints) -{ - if (endpoints.empty()) { - return Rosbag2QoS{}; - } - size_t num_endpoints = endpoints.size(); - size_t reliability_reliable_endpoints_count = 0; - size_t durability_transient_local_endpoints_count = 0; - for (const auto & endpoint : endpoints) { - const auto & profile = endpoint.qos_profile().get_rmw_qos_profile(); - if (profile.reliability == RMW_QOS_POLICY_RELIABILITY_RELIABLE) { - reliability_reliable_endpoints_count++; - } - if (profile.durability == RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL) { - durability_transient_local_endpoints_count++; - } - } - - bool service_event_topic = - rosbag2_cpp::is_service_event_topic(topic_name, endpoints[0].topic_type()); - - // We set policies in order as defined in rmw_qos_profile_t - Rosbag2QoS request_qos{}; - // Policy: history, depth - // History does not affect compatibility - - // Policy: reliability - if (reliability_reliable_endpoints_count == num_endpoints) { - request_qos.reliable(); - } else { - if (reliability_reliable_endpoints_count > 0) { - ROSBAG2_TRANSPORT_LOG_WARN_STREAM( - "Some, but not all, publishers on topic \"" << topic_name << "\" " - "are offering RMW_QOS_POLICY_RELIABILITY_RELIABLE. " - "Falling back to RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT " - "as it will connect to all publishers. " - "Some messages from Reliable publishers could be dropped."); - } - request_qos.best_effort(); - } - - // Policy: durability - // If all publishers offer transient_local, we can request it and receive latched messages - // - // If topic is for service event, rosbag doesn't want to receive latched messages (such as - // last response message). So only use durability_volatile. - if (!service_event_topic && durability_transient_local_endpoints_count == num_endpoints) { - request_qos.transient_local(); - } else { - if (!service_event_topic && durability_transient_local_endpoints_count > 0) { - ROSBAG2_TRANSPORT_LOG_WARN_STREAM( - "Some, but not all, publishers on topic \"" << topic_name << "\" " - "are offering RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL. " - "Falling back to RMW_QOS_POLICY_DURABILITY_VOLATILE " - "as it will connect to all publishers. " - "Previously-published latched messages will not be retrieved."); - } - request_qos.durability_volatile(); - } - // Policy: deadline - // Deadline does not affect delivery of messages, - // and we do not record Deadline"Missed events. - // We can always use unspecified deadline, which will be compatible with all publishers. - - // Policy: lifespan - // Lifespan does not affect compatibiliy - - // Policy: liveliness, liveliness_lease_duration - // Liveliness does not affect delivery of messages, - // and we do not record LivelinessChanged events. - // We can always use unspecified liveliness, which will be compatible with all publishers. - return request_qos; -} - -namespace -{ -bool operator==(const rmw_time_t & lhs, const rmw_time_t & rhs) -{ - return lhs.sec == rhs.sec && lhs.nsec == rhs.nsec; -} - -/** Check if all QoS profiles in the vector are identical when only looking at - * policies that affect compatibility. - * This means it excludes history and lifespan from the equality check. - */ -bool all_profiles_effectively_same(const std::vector & profiles) -{ - auto iterator = profiles.begin(); - const auto p_ref = iterator->get_rmw_qos_profile(); - iterator++; - for (; iterator != profiles.end(); iterator++) { - const auto p_next = iterator->get_rmw_qos_profile(); - bool compatibility_equals_previous = ( - // excluding history - p_ref.reliability == p_next.reliability && - p_ref.durability == p_next.durability && - p_ref.deadline == p_next.deadline && - // excluding lifespan - p_ref.liveliness == p_next.liveliness && - p_ref.liveliness_lease_duration == p_next.liveliness_lease_duration - ); - if (!compatibility_equals_previous) { - return false; - } - } - return true; -} -} // unnamed namespace - -Rosbag2QoS Rosbag2QoS::adapt_offer_to_recorded_offers( - const std::string & topic_name, const std::vector & profiles) -{ - if (profiles.empty()) { - return Rosbag2QoS{}; - } - if (all_profiles_effectively_same(profiles)) { - auto result = profiles[0]; - return result.default_history(); - } - - ROSBAG2_TRANSPORT_LOG_WARN_STREAM( - "Not all original publishers on topic " << topic_name << " offered the same QoS profiles. " - "Rosbag2 cannot yet choose an adapted profile to offer for this mixed case. " - "Falling back to the rosbag2_transport default publisher offer."); - return Rosbag2QoS{}; -} -} // namespace rosbag2_transport From 109403619cb28c3e842353c9382ffe64e71c9953 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 12 Dec 2023 16:42:06 +0800 Subject: [PATCH 28/35] Fix the path of a library Signed-off-by: Barry Xu --- rosbag2_py/CMakeLists.txt | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/rosbag2_py/CMakeLists.txt b/rosbag2_py/CMakeLists.txt index 06b2316abf..681e605fbb 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -48,11 +48,11 @@ target_link_libraries(_compression_options PUBLIC rosbag2_compression::rosbag2_compression ) -add_library(_format_output SHARED +add_library(rosbag2_format_output SHARED src/rosbag2_py/format_bag_metadata.cpp src/rosbag2_py/format_service_info.cpp ) -target_link_libraries(_format_output PUBLIC +target_link_libraries(rosbag2_format_output PUBLIC rosbag2_cpp::rosbag2_cpp ) @@ -71,7 +71,7 @@ pybind11_add_module(_storage SHARED target_link_libraries(_storage PUBLIC rosbag2_cpp::rosbag2_cpp rosbag2_storage::rosbag2_storage - _format_output + rosbag2_format_output ) pybind11_add_module(_writer SHARED @@ -89,7 +89,7 @@ pybind11_add_module(_info SHARED target_link_libraries(_info PUBLIC rosbag2_cpp::rosbag2_cpp rosbag2_storage::rosbag2_storage - _format_output + rosbag2_format_output ) pybind11_add_module(_transport SHARED @@ -114,7 +114,6 @@ target_link_libraries(_reindexer PUBLIC install( TARGETS _compression_options - _format_output _reader _storage _writer @@ -124,6 +123,11 @@ install( DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" ) +install( + TARGETS rosbag2_format_output + LIBRARY DESTINATION lib +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() From cd4173e9a284683396c1b264c0946c8729385383 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 14 Dec 2023 09:33:05 +0800 Subject: [PATCH 29/35] Resolve the conflicts caused by the rebase Signed-off-by: Barry Xu --- ros2bag/ros2bag/api/__init__.py | 3 +-- .../include/rosbag2_cpp/service_utils.hpp | 4 +++ rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 17 +++++++++++++ .../test/rosbag2_cpp/test_service_utils.cpp | 17 +++++++++++++ .../config_options_from_node_params.cpp | 25 +++++++++++++++++-- .../src/rosbag2_transport/recorder.cpp | 18 +++++++++++++ .../test/resources/recorder_node_params.yaml | 8 ++++-- .../test_composable_recorder.cpp | 12 +++++++-- 8 files changed, 96 insertions(+), 8 deletions(-) diff --git a/ros2bag/ros2bag/api/__init__.py b/ros2bag/ros2bag/api/__init__.py index 5fdcb55a8e..1bc91c5042 100644 --- a/ros2bag/ros2bag/api/__init__.py +++ b/ros2bag/ros2bag/api/__init__.py @@ -217,7 +217,6 @@ def convert_service_to_service_event_topic(services): return services_event_topics for service in services: - name = '/' + service if service[0] != '/' else service - services_event_topics.append(name + '/_service_event') + services_event_topics.append(service + '/_service_event') return services_event_topics diff --git a/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp index 340f014319..8bab8cd175 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp @@ -38,6 +38,10 @@ service_event_topic_type_to_service_type(const std::string & topic_type); ROSBAG2_CPP_PUBLIC size_t get_serialization_size_for_service_metadata_event(); + +ROSBAG2_CPP_PUBLIC +std::string +service_name_to_service_event_topic_name(const std::string & service_name); } // namespace rosbag2_cpp #endif // ROSBAG2_CPP__SERVICE_UTILS_HPP_ diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp index b6a4447108..e637b61336 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -139,4 +139,21 @@ size_t get_serialization_size_for_service_metadata_event() return size; } +std::string service_name_to_service_event_topic_name(const std::string & service_name) +{ + if (service_name.empty()) { + return service_name; + } + + // If the end of string is RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX, do nothing + if ((service_name.length() > sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) && + (service_name.substr(service_name.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) == + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) + { + return service_name; + } + + return service_name + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX; +} + } // namespace rosbag2_cpp diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp index 51420e20ac..c1dfdc531c 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp @@ -74,3 +74,20 @@ TEST_F(ServiceUtilsTest, check_service_event_topic_type_to_service_type) ); } } + +TEST_F(ServiceUtilsTest, check_service_name_to_service_event_topic_name) +{ + std::vector> all_test_data = + { + {"", ""}, + {"/a/_service_event", "/a/_service_event"}, + {"/abc", "/abc/_service_event"} + }; + + for (const auto & test_data : all_test_data) { + EXPECT_EQ( + rosbag2_cpp::service_name_to_service_event_topic_name(test_data.first), + test_data.second + ); + } +} diff --git a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp index 49dc587ac5..b9bf537070 100644 --- a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp +++ b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp @@ -16,6 +16,7 @@ #include #include "rclcpp/logging.hpp" +#include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/qos.hpp" #include "rosbag2_transport/play_options.hpp" #include "rosbag2_transport/config_options_from_node_params.hpp" @@ -204,7 +205,8 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node) RecordOptions get_record_options_from_node_params(rclcpp::Node & node) { RecordOptions record_options{}; - record_options.all = node.declare_parameter("record.all", false); + record_options.all_topics = node.declare_parameter("record.all_topics", false); + record_options.all_services = node.declare_parameter("record.all_services", false); record_options.is_discovery_disabled = node.declare_parameter("record.is_discovery_disabled", false); @@ -212,6 +214,25 @@ RecordOptions get_record_options_from_node_params(rclcpp::Node & node) record_options.topics = node.declare_parameter>( "record.topics", std::vector()); + // Convert service name to service event topic name + auto service_list = node.declare_parameter>( + "record.services", std::vector()); + for (auto & service : service_list) { + service = rosbag2_cpp::service_name_to_service_event_topic_name(service); + } + record_options.services = service_list; + + record_options.exclude_topics = node.declare_parameter>( + "record.exclude_topics", std::vector()); + + // Convert service name to service event topic name + auto exclude_service_list = node.declare_parameter>( + "record.exclude_services", std::vector()); + for (auto & service : exclude_service_list) { + service = rosbag2_cpp::service_name_to_service_event_topic_name(service); + } + record_options.exclude_service_events = exclude_service_list; + record_options.rmw_serialization_format = node.declare_parameter("record.rmw_serialization_format", "cdr"); @@ -220,7 +241,7 @@ RecordOptions get_record_options_from_node_params(rclcpp::Node & node) 0, 1000000).to_chrono(); record_options.regex = node.declare_parameter("record.regex", ""); - record_options.exclude = node.declare_parameter("record.exclude", ""); + record_options.exclude_regex = node.declare_parameter("record.exclude_regex", ""); record_options.node_prefix = node.declare_parameter("record.node_prefix", ""); record_options.compression_mode = node.declare_parameter( "record.compression_mode", diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index b1b3dee19d..8c09941b39 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -186,6 +186,24 @@ RecorderImpl::RecorderImpl( topic, node->get_name(), node->get_namespace(), false); } + + for (auto & exclude_topic : record_options_.exclude_topics) { + exclude_topic = rclcpp::expand_topic_or_service_name( + exclude_topic, node->get_name(), + node->get_namespace(), false); + } + + for (auto & service : record_options_.services) { + service = rclcpp::expand_topic_or_service_name( + service, node->get_name(), + node->get_namespace(), false); + } + + for (auto & exclude_service_event_topic : record_options_.exclude_service_events) { + exclude_service_event_topic = rclcpp::expand_topic_or_service_name( + exclude_service_event_topic, node->get_name(), + node->get_namespace(), false); + } } RecorderImpl::~RecorderImpl() diff --git a/rosbag2_transport/test/resources/recorder_node_params.yaml b/rosbag2_transport/test/resources/recorder_node_params.yaml index b5ca93b7b1..b37a41e2d9 100644 --- a/rosbag2_transport/test/resources/recorder_node_params.yaml +++ b/rosbag2_transport/test/resources/recorder_node_params.yaml @@ -4,15 +4,19 @@ recorder_params_node: use_sim_time: false record: - all: true + all_topics: true + all_services: true is_discovery_disabled: true topics: ["topic", "other_topic"] + services: ["service", "other_service"] rmw_serialization_format: "cdr" topic_polling_interval: sec: 0 nsec: 10000000 regex: "[xyz]/topic" - exclude: "*" + exclude_regex: "(.*)" + exclude_topics: ["exclude_topic", "other_exclude_topic"] + exclude_services: ["exclude_service", "other_exclude_service"] node_prefix: "prefix" compression_mode: "stream" compression_format: "h264" diff --git a/rosbag2_transport/test/rosbag2_transport/test_composable_recorder.cpp b/rosbag2_transport/test/rosbag2_transport/test_composable_recorder.cpp index 54dbbe055e..aea1b950d9 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_composable_recorder.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_composable_recorder.cpp @@ -207,14 +207,22 @@ TEST_P(ComposableRecorderTests, recorder_can_parse_parameters_from_file) { auto record_options = recorder->get_record_options(); auto storage_options = recorder->get_storage_options(); - EXPECT_EQ(record_options.all, true); + EXPECT_EQ(record_options.all_topics, true); + EXPECT_EQ(record_options.all_services, true); EXPECT_EQ(record_options.is_discovery_disabled, true); std::vector topics {"/topic", "/other_topic"}; EXPECT_EQ(record_options.topics, topics); + std::vector services {"/service/_service_event", "/other_service/_service_event"}; + EXPECT_EQ(record_options.services, services); EXPECT_EQ(record_options.rmw_serialization_format, "cdr"); EXPECT_TRUE(record_options.topic_polling_interval == 0.01s); EXPECT_EQ(record_options.regex, "[xyz]/topic"); - EXPECT_EQ(record_options.exclude, "*"); + EXPECT_EQ(record_options.exclude_regex, "(.*)"); + std::vector exclude_topics {"/exclude_topic", "/other_exclude_topic"}; + EXPECT_EQ(record_options.exclude_topics, exclude_topics); + std::vector exclude_services { + "/exclude_service/_service_event", "/other_exclude_service/_service_event"}; + EXPECT_EQ(record_options.exclude_service_events, exclude_services); EXPECT_EQ(record_options.node_prefix, "prefix"); EXPECT_EQ(record_options.compression_mode, "stream"); EXPECT_EQ(record_options.compression_format, "h264"); From 5c2006503d91ce47b29cf8b89782954fdf601d6b Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 14 Dec 2023 12:04:29 +0800 Subject: [PATCH 30/35] Replace sizeof() with strlen() Signed-off-by: Barry Xu --- rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp index e637b61336..786c38d8e8 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -31,12 +31,12 @@ 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)) { + if (topic.length() <= strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) { return false; } std::string end_topic_name = topic.substr( - topic.length() - (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)); + topic.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)); // Should be "/_service_event" if (end_topic_name != RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) { @@ -65,20 +65,20 @@ bool is_service_event_topic(const std::string & topic, const std::string & topic std::string service_event_topic_name_to_service_name(const std::string & topic_name) { std::string service_name; - if (topic_name.length() <= (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)) { + if (topic_name.length() <= strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) { return service_name; } if (topic_name.substr( topic_name.length() - - (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)) != + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) != RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) { return service_name; } service_name = topic_name.substr( - 0, topic_name.length() - (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)); + 0, topic_name.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)); return service_name; } @@ -146,7 +146,7 @@ std::string service_name_to_service_event_topic_name(const std::string & service } // If the end of string is RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX, do nothing - if ((service_name.length() > sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) && + if ((service_name.length() > strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) && (service_name.substr(service_name.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) == RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) { From 097384066e5a5e626c683e7f8070fd98c2b887dc Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 14 Dec 2023 13:24:07 +0800 Subject: [PATCH 31/35] Use consistent variable name Signed-off-by: Barry Xu --- ros2bag/ros2bag/verb/record.py | 2 +- rosbag2_py/src/rosbag2_py/_transport.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index e2d5e484c4..9c9ffd098c 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -288,7 +288,7 @@ def main(self, *, args): # noqa: D102 record_options.regex = args.regex record_options.exclude_regex = args.exclude_regex record_options.exclude_topics = args.exclude_topics if args.exclude_topics else [] - record_options.exclude_services = \ + record_options.exclude_service_events = \ convert_service_to_service_event_topic(args.exclude_services) record_options.node_prefix = NODE_NAME_PREFIX record_options.compression_mode = args.compression_mode diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 0e3cb12afb..6593b75ae4 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -358,7 +358,7 @@ PYBIND11_MODULE(_transport, m) { .def_readwrite("regex", &RecordOptions::regex) .def_readwrite("exclude_regex", &RecordOptions::exclude_regex) .def_readwrite("exclude_topics", &RecordOptions::exclude_topics) - .def_readwrite("exclude_services", &RecordOptions::exclude_service_events) + .def_readwrite("exclude_service_events", &RecordOptions::exclude_service_events) .def_readwrite("node_prefix", &RecordOptions::node_prefix) .def_readwrite("compression_mode", &RecordOptions::compression_mode) .def_readwrite("compression_format", &RecordOptions::compression_format) From 25653d0ccdf810de64a8151e33afe1c836c7e441 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 15 Dec 2023 09:44:43 +0800 Subject: [PATCH 32/35] Fix a link error on Windows Signed-off-by: Barry Xu --- rosbag2_py/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rosbag2_py/CMakeLists.txt b/rosbag2_py/CMakeLists.txt index 681e605fbb..2629e8fa88 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -126,6 +126,8 @@ install( install( TARGETS rosbag2_format_output LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin ) if(BUILD_TESTING) From 6543244a6689f1c15316bc90b59e1840599e0a96 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 20 Dec 2023 10:11:44 +0800 Subject: [PATCH 33/35] Use visibility control for format library Signed-off-by: Barry Xu --- rosbag2_py/CMakeLists.txt | 13 ++--- .../src/rosbag2_py/format_bag_metadata.hpp | 3 +- .../src/rosbag2_py/format_service_info.hpp | 3 +- .../src/rosbag2_py/visibility_control.hpp | 58 +++++++++++++++++++ 4 files changed, 68 insertions(+), 9 deletions(-) create mode 100644 rosbag2_py/src/rosbag2_py/visibility_control.hpp diff --git a/rosbag2_py/CMakeLists.txt b/rosbag2_py/CMakeLists.txt index 2629e8fa88..d4d4b2de79 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -55,6 +55,12 @@ add_library(rosbag2_format_output SHARED target_link_libraries(rosbag2_format_output PUBLIC rosbag2_cpp::rosbag2_cpp ) +install( + TARGETS rosbag2_format_output + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) pybind11_add_module(_reader SHARED src/rosbag2_py/_reader.cpp @@ -123,13 +129,6 @@ install( DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" ) -install( - TARGETS rosbag2_format_output - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin -) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp index ba2e868ed7..bfc9fa5c29 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp @@ -18,8 +18,9 @@ #include #include "rosbag2_storage/bag_metadata.hpp" +#include "visibility_control.hpp" -std::string format_bag_meta_data( +ROSBAG2_PY_PUBLIC 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.hpp b/rosbag2_py/src/rosbag2_py/format_service_info.hpp index 7337e5be4e..d91269b68f 100644 --- a/rosbag2_py/src/rosbag2_py/format_service_info.hpp +++ b/rosbag2_py/src/rosbag2_py/format_service_info.hpp @@ -20,8 +20,9 @@ #include #include "rosbag2_cpp/info.hpp" +#include "visibility_control.hpp" -std::string format_service_info( +ROSBAG2_PY_PUBLIC std::string format_service_info( std::vector> & service_info); #endif // ROSBAG2_PY__FORMAT_SERVICE_INFO_HPP_ diff --git a/rosbag2_py/src/rosbag2_py/visibility_control.hpp b/rosbag2_py/src/rosbag2_py/visibility_control.hpp new file mode 100644 index 0000000000..5b7f557b00 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/visibility_control.hpp @@ -0,0 +1,58 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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__VISIBILITY_CONTROL_HPP_ +#define ROSBAG2_PY__VISIBILITY_CONTROL_HPP_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ROSBAG2_PY_EXPORT __attribute__ ((dllexport)) + #define ROSBAG2_PY_IMPORT __attribute__ ((dllimport)) + #else + #define ROSBAG2_PY_EXPORT __declspec(dllexport) + #define ROSBAG2_PY_IMPORT __declspec(dllimport) + #endif + #ifdef ROSBAG2_PY_BUILDING_DLL + #define ROSBAG2_PY_PUBLIC ROSBAG2_PY_EXPORT + #else + #define ROSBAG2_PY_PUBLIC ROSBAG2_PY_IMPORT + #endif + #define ROSBAG2_PY_PUBLIC_TYPE ROSBAG2_PY_PUBLIC + #define ROSBAG2_PY_LOCAL +#else + #define ROSBAG2_PY_EXPORT __attribute__ ((visibility("default"))) + #define ROSBAG2_PY_IMPORT + #if __GNUC__ >= 4 + #define ROSBAG2_PY_PUBLIC __attribute__ ((visibility("default"))) + #define ROSBAG2_PY_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define ROSBAG2_PY_PUBLIC + #define ROSBAG2_PY_LOCAL + #endif + #define ROSBAG2_PY_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // ROSBAG2_PY__VISIBILITY_CONTROL_HPP_ From 642d50bf931a64a7685caaf207bbeb6beccbc496 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 20 Dec 2023 16:31:20 -0800 Subject: [PATCH 34/35] Replace rosbag2_format_output by direct source files linkage - Also put `format_service_info(..)` and `format_bag_meta_data(..)` under the rosbag2_py namespace Signed-off-by: Michael Orlov --- rosbag2_py/CMakeLists.txt | 22 +++++-------------- rosbag2_py/src/rosbag2_py/_info.cpp | 2 +- rosbag2_py/src/rosbag2_py/_storage.cpp | 8 +++---- .../src/rosbag2_py/format_bag_metadata.cpp | 5 +++++ .../src/rosbag2_py/format_bag_metadata.hpp | 8 +++++-- .../src/rosbag2_py/format_service_info.cpp | 5 +++++ .../src/rosbag2_py/format_service_info.hpp | 8 +++++-- 7 files changed, 32 insertions(+), 26 deletions(-) diff --git a/rosbag2_py/CMakeLists.txt b/rosbag2_py/CMakeLists.txt index d4d4b2de79..1ab3d13ecc 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -48,20 +48,6 @@ target_link_libraries(_compression_options PUBLIC rosbag2_compression::rosbag2_compression ) -add_library(rosbag2_format_output SHARED - src/rosbag2_py/format_bag_metadata.cpp - src/rosbag2_py/format_service_info.cpp -) -target_link_libraries(rosbag2_format_output PUBLIC - rosbag2_cpp::rosbag2_cpp -) -install( - TARGETS rosbag2_format_output - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin -) - pybind11_add_module(_reader SHARED src/rosbag2_py/_reader.cpp ) @@ -73,11 +59,12 @@ target_link_libraries(_reader PUBLIC pybind11_add_module(_storage SHARED src/rosbag2_py/_storage.cpp + src/rosbag2_py/format_bag_metadata.hpp + src/rosbag2_py/format_bag_metadata.cpp ) target_link_libraries(_storage PUBLIC rosbag2_cpp::rosbag2_cpp rosbag2_storage::rosbag2_storage - rosbag2_format_output ) pybind11_add_module(_writer SHARED @@ -91,11 +78,14 @@ target_link_libraries(_writer PUBLIC pybind11_add_module(_info SHARED src/rosbag2_py/_info.cpp + src/rosbag2_py/format_bag_metadata.hpp + src/rosbag2_py/format_bag_metadata.cpp + src/rosbag2_py/format_service_info.hpp + src/rosbag2_py/format_service_info.cpp ) target_link_libraries(_info PUBLIC rosbag2_cpp::rosbag2_cpp rosbag2_storage::rosbag2_storage - rosbag2_format_output ) pybind11_add_module(_transport SHARED diff --git a/rosbag2_py/src/rosbag2_py/_info.cpp b/rosbag2_py/src/rosbag2_py/_info.cpp index f6e0f428a1..714a570066 100644 --- a/rosbag2_py/src/rosbag2_py/_info.cpp +++ b/rosbag2_py/src/rosbag2_py/_info.cpp @@ -20,7 +20,7 @@ #include "rosbag2_cpp/info.hpp" #include "rosbag2_storage/bag_metadata.hpp" -#include "./pybind11.hpp" +#include "pybind11.hpp" namespace rosbag2_py { diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index 8d9c7285ec..db0c150db3 100644 --- a/rosbag2_py/src/rosbag2_py/_storage.cpp +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -23,11 +23,9 @@ #include "rosbag2_storage/storage_interfaces/base_read_interface.hpp" #include "rosbag2_storage/storage_options.hpp" #include "rosbag2_storage/topic_metadata.hpp" -#include "rosbag2_storage/qos.hpp" -#include "./format_bag_metadata.hpp" - -#include "./pybind11.hpp" +#include "format_bag_metadata.hpp" +#include "pybind11.hpp" namespace { @@ -358,7 +356,7 @@ PYBIND11_MODULE(_storage, m) { .def_readwrite("ros_distro", &rosbag2_storage::BagMetadata::ros_distro) .def( "__repr__", [](const rosbag2_storage::BagMetadata & metadata) { - return format_bag_meta_data(metadata); + return rosbag2_py::format_bag_meta_data(metadata); }); pybind11::enum_(m, "ReadOrderSortBy") diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp index c7ed84645d..074cf332c3 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp @@ -223,6 +223,9 @@ void format_service_with_type( } // namespace +namespace rosbag2_py +{ + std::string format_bag_meta_data( const rosbag2_storage::BagMetadata & metadata, bool only_topic) @@ -270,3 +273,5 @@ std::string format_bag_meta_data( return info_stream.str(); } + +} // namespace rosbag2_py diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp index bfc9fa5c29..dabd871f93 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp @@ -18,9 +18,13 @@ #include #include "rosbag2_storage/bag_metadata.hpp" -#include "visibility_control.hpp" -ROSBAG2_PY_PUBLIC std::string format_bag_meta_data( +namespace rosbag2_py +{ + +std::string format_bag_meta_data( const rosbag2_storage::BagMetadata & metadata, bool only_topic = false); +} // namespace rosbag2_py + #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 index aec57a0550..a14ee89e01 100644 --- a/rosbag2_py/src/rosbag2_py/format_service_info.cpp +++ b/rosbag2_py/src/rosbag2_py/format_service_info.cpp @@ -16,6 +16,9 @@ #include "format_service_info.hpp" +namespace rosbag2_py +{ + std::string format_service_info( std::vector> & service_info_list) @@ -49,3 +52,5 @@ format_service_info( return info_stream.str(); } + +} // namespace rosbag2_py diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.hpp b/rosbag2_py/src/rosbag2_py/format_service_info.hpp index d91269b68f..d5cb5fc788 100644 --- a/rosbag2_py/src/rosbag2_py/format_service_info.hpp +++ b/rosbag2_py/src/rosbag2_py/format_service_info.hpp @@ -20,9 +20,13 @@ #include #include "rosbag2_cpp/info.hpp" -#include "visibility_control.hpp" -ROSBAG2_PY_PUBLIC std::string format_service_info( +namespace rosbag2_py +{ + +std::string format_service_info( std::vector> & service_info); +} // namespace rosbag2_py + #endif // ROSBAG2_PY__FORMAT_SERVICE_INFO_HPP_ From e8786144dcd6be0b51a5046fddfaf7d33246368e Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Wed, 20 Dec 2023 22:39:12 -0800 Subject: [PATCH 35/35] Remove header files from pybind modules and delete visibility_control.hpp Signed-off-by: Michael Orlov --- rosbag2_py/CMakeLists.txt | 3 - .../src/rosbag2_py/visibility_control.hpp | 58 ------------------- 2 files changed, 61 deletions(-) delete mode 100644 rosbag2_py/src/rosbag2_py/visibility_control.hpp diff --git a/rosbag2_py/CMakeLists.txt b/rosbag2_py/CMakeLists.txt index 1ab3d13ecc..97e74f24f9 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -59,7 +59,6 @@ target_link_libraries(_reader PUBLIC pybind11_add_module(_storage SHARED src/rosbag2_py/_storage.cpp - src/rosbag2_py/format_bag_metadata.hpp src/rosbag2_py/format_bag_metadata.cpp ) target_link_libraries(_storage PUBLIC @@ -78,9 +77,7 @@ target_link_libraries(_writer PUBLIC pybind11_add_module(_info SHARED src/rosbag2_py/_info.cpp - src/rosbag2_py/format_bag_metadata.hpp src/rosbag2_py/format_bag_metadata.cpp - src/rosbag2_py/format_service_info.hpp src/rosbag2_py/format_service_info.cpp ) target_link_libraries(_info PUBLIC diff --git a/rosbag2_py/src/rosbag2_py/visibility_control.hpp b/rosbag2_py/src/rosbag2_py/visibility_control.hpp deleted file mode 100644 index 5b7f557b00..0000000000 --- a/rosbag2_py/src/rosbag2_py/visibility_control.hpp +++ /dev/null @@ -1,58 +0,0 @@ -// Copyright 2018 Open Source Robotics Foundation, Inc. -// -// 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__VISIBILITY_CONTROL_HPP_ -#define ROSBAG2_PY__VISIBILITY_CONTROL_HPP_ - -#ifdef __cplusplus -extern "C" -{ -#endif - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define ROSBAG2_PY_EXPORT __attribute__ ((dllexport)) - #define ROSBAG2_PY_IMPORT __attribute__ ((dllimport)) - #else - #define ROSBAG2_PY_EXPORT __declspec(dllexport) - #define ROSBAG2_PY_IMPORT __declspec(dllimport) - #endif - #ifdef ROSBAG2_PY_BUILDING_DLL - #define ROSBAG2_PY_PUBLIC ROSBAG2_PY_EXPORT - #else - #define ROSBAG2_PY_PUBLIC ROSBAG2_PY_IMPORT - #endif - #define ROSBAG2_PY_PUBLIC_TYPE ROSBAG2_PY_PUBLIC - #define ROSBAG2_PY_LOCAL -#else - #define ROSBAG2_PY_EXPORT __attribute__ ((visibility("default"))) - #define ROSBAG2_PY_IMPORT - #if __GNUC__ >= 4 - #define ROSBAG2_PY_PUBLIC __attribute__ ((visibility("default"))) - #define ROSBAG2_PY_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define ROSBAG2_PY_PUBLIC - #define ROSBAG2_PY_LOCAL - #endif - #define ROSBAG2_PY_PUBLIC_TYPE -#endif - -#ifdef __cplusplus -} -#endif - -#endif // ROSBAG2_PY__VISIBILITY_CONTROL_HPP_