diff --git a/docs/message_definition_encoding.md b/docs/message_definition_encoding.md index 17e48fdb30..a3f5d950d5 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,19 @@ MSG: my_msgs/msg/BasicMsg float32 my_float ``` +Another example is a service message definition for `my_msgs/srv/ExampleSrv` in `ros2msg` form + +``` +# defines a service message that includes a field of a custom message type +my_msgs/BasicMsg request +--- +my_msgs/BasicMsg response +================================================================================ +MSG: my_msgs/msg/BasicMsg +# defines a message with a primitive type field +float32 my_float +``` + ## `ros2idl` encoding The IDL definition of the type specified by name along with all dependent types are stored together. The IDL definitions can be stored in any order. Every definition is preceded by a two-line delimiter: diff --git a/ros2bag/ros2bag/api/__init__.py b/ros2bag/ros2bag/api/__init__.py index 0c9a192ed2..1bc91c5042 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,15 @@ def add_writer_storage_plugin_extensions(parser: ArgumentParser) -> None: 'Settings in this profile can still be overridden by other explicit options ' 'and --storage-config-file. Profiles:\n' + '\n'.join([f'{preset[0]}: {preset[1]}' for preset in preset_profiles])) + + +def convert_service_to_service_event_topic(services): + services_event_topics = [] + + if not services: + return services_event_topics + + for service in services: + services_event_topics.append(service + '/_service_event') + + return services_event_topics diff --git a/ros2bag/ros2bag/verb/info.py b/ros2bag/ros2bag/verb/info.py index 7b78730edd..160f128c8c 100644 --- a/ros2bag/ros2bag/verb/info.py +++ b/ros2bag/ros2bag/verb/info.py @@ -26,11 +26,38 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '-t', '--topic-name', action='store_true', help='Only display topic names.' ) + parser.add_argument( + '-v', '--verbose', action='store_true', + help='Display request/response information for services' + ) + + def _is_service_event_topic(self, topic_name, topic_type) -> bool: + + service_event_type_middle = '/srv/' + service_event_type_postfix = '_Event' + + if (service_event_type_middle not in topic_type + or not topic_type.endswith(service_event_type_postfix)): + return False + + service_event_topic_postfix = '/_service_event' + if not topic_name.endswith(service_event_topic_postfix): + return False + + return True def main(self, *, args): # noqa: D102 - m = Info().read_metadata(args.bag_path, args.storage) - if args.topic_name: - for topic_info in m.topics_with_message_count: - print(topic_info.topic_metadata.name) + if args.topic_name and args.verbose: + print("Warning! You have set both the '-t' and '-v' parameters. The '-t' parameter " + 'will be ignored.') + if args.verbose: + Info().read_metadata_and_output_service_verbose(args.bag_path, args.storage) else: - print(m) + m = Info().read_metadata(args.bag_path, args.storage) + if args.topic_name: + for topic_info in m.topics_with_message_count: + if not self._is_service_event_topic(topic_info.topic_metadata.name, + topic_info.topic_metadata.type): + print(topic_info.topic_metadata.name) + else: + print(m) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 8c49c4b679..9c9ffd098c 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,35 @@ def add_arguments(self, parser, cli_name): # noqa: D102 'topics', nargs='*', default=None, help='List of topics to record.') parser.add_argument( '-a', '--all', action='store_true', - help='Record all topics. Required if no explicit topic list or regex filters.') + help='Record all topics and services (Exclude hidden topic).') + parser.add_argument( + '--all-topics', action='store_true', + help='Record all topics (Exclude hidden topic).') + parser.add_argument( + '--all-services', action='store_true', + help='Record all services via service event topics.') parser.add_argument( '-e', '--regex', default='', - help='Record only topics containing provided regular expression. ' - 'Overrides --all, applies on top of topics list.') + help='Record only topics and services containing provided regular expression. ' + 'Overrides --all, --all-topics and --all-services, applies on top of ' + 'topics list and service list.') + parser.add_argument( + '--exclude-regex', default='', + help='Exclude topics and services containing provided regular expression. ' + 'Works on top of --all, --all-topics, or --regex.') + parser.add_argument( + '--exclude-topics', type=str, metavar='Topic', nargs='+', + help='List of topics not being recorded. ' + 'Works on top of --all, --all-topics, or --regex.') + parser.add_argument( + '--exclude-services', type=str, metavar='ServiceName', nargs='+', + help='List of services not being recorded. ' + 'Works on top of --all, --all-services, or --regex.') + + # Enable to record service parser.add_argument( - '-x', '--exclude', default='', - help='Exclude topics containing provided regular expression. ' - 'Works on top of --all, --regex, or topics list.') + '--services', type=str, metavar='ServiceName', nargs='+', + help='List of services to record.') # Discovery behavior parser.add_argument( @@ -167,20 +188,46 @@ def add_arguments(self, parser, cli_name): # noqa: D102 help='Choose the compression format/algorithm. ' 'Has no effect if no compression mode is chosen. Default: %(default)s.') + def _check_necessary_argument(self, args): + # At least one options out of --all, --all-topics, --all-services, --services, --topics or + # --regex must be used + if not (args.all or args.all_topics or args.all_services or + args.services or (args.topics and len(args.topics) > 0) or args.regex): + return False + return True + def main(self, *, args): # noqa: D102 - # both all and topics cannot be true - if (args.all and (args.topics or args.regex)) or (args.topics and args.regex): - return print_error('Must specify only one option out of topics, --regex or --all') - # one out of "all", "topics" and "regex" must be true - if not(args.all or (args.topics and len(args.topics) > 0) or (args.regex)): - return print_error('Invalid choice: Must specify topic(s), --regex or --all') - if args.topics and args.exclude: - return print_error('--exclude argument cannot be used when specifying a list ' - 'of topics explicitly') + if not self._check_necessary_argument(args): + return print_error('Need to specify one option out of --all, --all-topics, ' + '--all-services, --services, topics and --regex') + + # Only one option out of --all, --all-services --services or --regex can be used + if (args.all and args.all_services) or \ + ((args.all or args.all_services) and args.regex) or \ + ((args.all or args.all_services or args.regex) and args.services): + return print_error('Must specify only one option out of --all, --all-services, ' + '--services or --regex') + + # Only one option out of --all, --all-topics, topics or --regex can be used + if (args.all and args.all_topics) or \ + ((args.all or args.all_topics) and args.regex) or \ + ((args.all or args.all_topics or args.regex) and args.topics): + return print_error('Must specify only one option out of --all, --all-topics, ' + 'topics or --regex') - if args.exclude and not(args.regex or args.all): - return print_error('--exclude argument requires either --all or --regex') + if (args.exclude_regex and + not (args.regex or args.all or args.all_topics or args.all_services)): + return print_error('--exclude-regex argument requires either --all, ' + '--all-topics, --all-services or --regex') + + if args.exclude_topics and not (args.regex or args.all or args.all_topics): + return print_error('--exclude-topics argument requires either --all, --all-topics ' + 'or --regex') + + if args.exclude_services and not (args.regex or args.all or args.all_services): + return print_error('--exclude-services argument requires either --all, --all-services ' + 'or --regex') uri = args.output or datetime.datetime.now().strftime('rosbag2_%Y_%m_%d-%H_%M_%S') @@ -232,14 +279,17 @@ def main(self, *, args): # noqa: D102 custom_data=custom_data ) record_options = RecordOptions() - record_options.all = args.all + record_options.all_topics = args.all_topics or args.all record_options.is_discovery_disabled = args.no_discovery record_options.topics = args.topics record_options.rmw_serialization_format = args.serialization_format record_options.topic_polling_interval = datetime.timedelta( milliseconds=args.polling_interval) record_options.regex = args.regex - record_options.exclude = args.exclude + record_options.exclude_regex = args.exclude_regex + record_options.exclude_topics = args.exclude_topics if args.exclude_topics else [] + record_options.exclude_service_events = \ + convert_service_to_service_event_topic(args.exclude_services) record_options.node_prefix = NODE_NAME_PREFIX record_options.compression_mode = args.compression_mode record_options.compression_format = args.compression_format @@ -251,6 +301,10 @@ def main(self, *, args): # noqa: D102 record_options.start_paused = args.start_paused record_options.ignore_leaf_topics = args.ignore_leaf_topics record_options.use_sim_time = args.use_sim_time + record_options.all_services = args.all_services or args.all + + # Convert service name to service event topic name + record_options.services = convert_service_to_service_event_topic(args.services) recorder = Recorder() 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..8bab8cd175 --- /dev/null +++ b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp @@ -0,0 +1,47 @@ +// Copyright 2023 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_CPP__SERVICE_UTILS_HPP_ +#define ROSBAG2_CPP__SERVICE_UTILS_HPP_ + +#include + +#include "rosbag2_cpp/visibility_control.hpp" + +namespace rosbag2_cpp +{ +ROSBAG2_CPP_PUBLIC +bool +is_service_event_topic(const std::string & topic, const std::string & topic_type); + +// Call this function after is_service_event_topic() return true +ROSBAG2_CPP_PUBLIC +std::string +service_event_topic_name_to_service_name(const std::string & topic_name); + +// Call this function after is_service_event_topic() return true +ROSBAG2_CPP_PUBLIC +std::string +service_event_topic_type_to_service_type(const std::string & topic_type); + +ROSBAG2_CPP_PUBLIC +size_t +get_serialization_size_for_service_metadata_event(); + +ROSBAG2_CPP_PUBLIC +std::string +service_name_to_service_event_topic_name(const std::string & service_name); +} // namespace rosbag2_cpp + +#endif // ROSBAG2_CPP__SERVICE_UTILS_HPP_ diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index 4d897c7aa9..9e00cb742c 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -14,14 +14,18 @@ #include "rosbag2_cpp/info.hpp" -#include +#include +#include #include #include +#include "rmw/rmw.h" +#include "rosidl_typesupport_cpp/message_type_support.hpp" #include "rcpputils/filesystem_helper.hpp" -#include "rosbag2_storage/logging.hpp" +#include "service_msgs/msg/service_event_info.hpp" + +#include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/metadata_io.hpp" -#include "rosbag2_storage/storage_interfaces/read_only_interface.hpp" #include "rosbag2_storage/storage_factory.hpp" namespace rosbag2_cpp @@ -52,4 +56,128 @@ rosbag2_storage::BagMetadata Info::read_metadata( return storage->get_metadata(); } +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; + 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 = service_msgs::msg::ServiceEventInfo::_client_gid_type; +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); + } + + 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>; + + 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( + "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..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 @@ -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 ) @@ -105,6 +105,15 @@ std::set parse_definition_dependencies( 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"); } @@ -117,6 +126,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 +145,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 +180,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 +199,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,7 +207,7 @@ 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; @@ -208,21 +224,62 @@ rosbag2_storage::MessageDefinition LocalMessageDefinitionSource::get_full_text( }; std::string result; - Format format = 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_topic_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); - 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) { @@ -230,6 +287,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 +296,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..786c38d8e8 --- /dev/null +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -0,0 +1,159 @@ +// 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.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() <= strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) { + return false; + } + + std::string end_topic_name = topic.substr( + topic.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)); + + // 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; + } + + // 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; +} + +std::string service_event_topic_name_to_service_name(const std::string & topic_name) +{ + std::string service_name; + if (topic_name.length() <= strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) { + return service_name; + } + + if (topic_name.substr( + topic_name.length() - + strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) != + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + { + return service_name; + } + + service_name = topic_name.substr( + 0, topic_name.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)); + + return service_name; +} + +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 service_type; + } + + // Should include '/srv/' in type + if (topic_type.find(service_event_topic_type_middle) == std::string::npos) { + return service_type; + } + + if (topic_type.substr(topic_type.length() - std::strlen(service_event_topic_type_postfix)) != + service_event_topic_type_postfix) + { + return service_type; + } + + 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) { + throw 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; +} + +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() > strlen(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/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/bags/only_services.mcap b/rosbag2_cpp/test/rosbag2_cpp/bags/only_services.mcap new file mode 100644 index 0000000000..4e8d72a0fe Binary files /dev/null and b/rosbag2_cpp/test/rosbag2_cpp/bags/only_services.mcap differ 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 0000000000..3a4abb7c63 Binary files /dev/null and b/rosbag2_cpp/test/rosbag2_cpp/bags/only_topics.mcap differ 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 0000000000..22714632fa Binary files /dev/null and b/rosbag2_cpp/test/rosbag2_cpp/bags/topics_and_services.db3 differ diff --git a/rosbag2_cpp/test/rosbag2_cpp/bags/topics_and_services.mcap b/rosbag2_cpp/test/rosbag2_cpp/bags/topics_and_services.mcap new file mode 100644 index 0000000000..4e39117bcb Binary files /dev/null and b/rosbag2_cpp/test/rosbag2_cpp/bags/topics_and_services.mcap differ 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..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,6 +62,51 @@ TEST(test_local_message_definition_source, can_find_msg_deps) "float32 c\n"); } +TEST(test_local_message_definition_source, can_find_srv_deps_in_msg) +{ + LocalMessageDefinitionSource source; + 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") << 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) { LocalMessageDefinitionSource source; @@ -132,7 +177,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..c1dfdc531c --- /dev/null +++ b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp @@ -0,0 +1,93 @@ +// 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 + ); + } +} + +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_py/CMakeLists.txt b/rosbag2_py/CMakeLists.txt index 1f90a8a171..97e74f24f9 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -77,6 +77,8 @@ target_link_libraries(_writer PUBLIC pybind11_add_module(_info SHARED src/rosbag2_py/_info.cpp + src/rosbag2_py/format_bag_metadata.cpp + src/rosbag2_py/format_service_info.cpp ) target_link_libraries(_info PUBLIC rosbag2_cpp::rosbag2_cpp diff --git a/rosbag2_py/src/rosbag2_py/_info.cpp b/rosbag2_py/src/rosbag2_py/_info.cpp index fdd7d00dc8..714a570066 100644 --- a/rosbag2_py/src/rosbag2_py/_info.cpp +++ b/rosbag2_py/src/rosbag2_py/_info.cpp @@ -15,10 +15,12 @@ #include #include +#include "format_bag_metadata.hpp" +#include "format_service_info.hpp" #include "rosbag2_cpp/info.hpp" #include "rosbag2_storage/bag_metadata.hpp" -#include "./pybind11.hpp" +#include "pybind11.hpp" namespace rosbag2_py { @@ -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/_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/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 3593fc9fd8..6593b75ae4 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -350,13 +350,15 @@ 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_regex", &RecordOptions::exclude_regex) + .def_readwrite("exclude_topics", &RecordOptions::exclude_topics) + .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) @@ -371,6 +373,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..074cf332c3 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,109 @@ 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) { + info_stream << std::endl; + 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 = 0; +}; + +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) +namespace rosbag2_py +{ + +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 +239,12 @@ 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; + 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 +257,21 @@ 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.empty()) { + format_service_with_type(service_info_list, info_stream, indentation_spaces + 2); + } + } + 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 30cd8d4344..dabd871f93 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp @@ -19,6 +19,12 @@ #include "rosbag2_storage/bag_metadata.hpp" -std::string format_bag_meta_data(const rosbag2_storage::BagMetadata & metadata); +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 new file mode 100644 index 0000000000..a14ee89e01 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/format_service_info.cpp @@ -0,0 +1,56 @@ +// 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" + +namespace rosbag2_py +{ + +std::string +format_service_info( + std::vector> & service_info_list) +{ + std::stringstream info_stream; + 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_info_string; + + 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(); +} + +} // 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 new file mode 100644 index 0000000000..d5cb5fc788 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/format_service_info.hpp @@ -0,0 +1,32 @@ +// 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" + +namespace rosbag2_py +{ + +std::string format_service_info( + std::vector> & service_info); + +} // namespace rosbag2_py + +#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..7189489578 --- /dev/null +++ b/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp @@ -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. + +#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 do_nothing_srv_callback = + [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_, do_nothing_srv_callback); + + 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 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; + } + + 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, timeout) != 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_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_ diff --git a/rosbag2_test_msgdefs/CMakeLists.txt b/rosbag2_test_msgdefs/CMakeLists.txt index 32199bf446..adacc5feab 100644 --- a/rosbag2_test_msgdefs/CMakeLists.txt +++ b/rosbag2_test_msgdefs/CMakeLists.txt @@ -15,6 +15,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ComplexMsg.msg" "msg/ComplexMsgDependsOnIdl.msg" "srv/BasicSrv.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/ComplexSrvMsg.srv b/rosbag2_test_msgdefs/srv/ComplexSrvMsg.srv new file mode 100644 index 0000000000..d987d100fd --- /dev/null +++ b/rosbag2_test_msgdefs/srv/ComplexSrvMsg.srv @@ -0,0 +1,3 @@ +rosbag2_test_msgdefs/BasicMsg req +--- +rosbag2_test_msgdefs/BasicMsg resp diff --git a/rosbag2_tests/CMakeLists.txt b/rosbag2_tests/CMakeLists.txt index 5ac04d58dc..d856d80dbc 100644 --- a/rosbag2_tests/CMakeLists.txt +++ b/rosbag2_tests/CMakeLists.txt @@ -120,6 +120,19 @@ if(BUILD_TESTING) ${test_msgs_TARGETS} ) endif() + + 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_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 + rosbag2_transport::rosbag2_transport + ${test_msgs_TARGETS} + ) + endif() endif() ament_package() 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/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 0000000000..e68cdbe4b7 Binary files /dev/null and b/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/bag_with_topics_and_service_events.mcap differ diff --git a/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/metadata.yaml b/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/metadata.yaml new file mode 100644 index 0000000000..9170884e3a --- /dev/null +++ b/rosbag2_tests/resources/mcap/bag_with_topics_and_service_events/metadata.yaml @@ -0,0 +1,57 @@ +rosbag2_bagfile_information: + version: 8 + storage_identifier: mcap + duration: + nanoseconds: 70653944 + starting_time: + nanoseconds_since_epoch: 1699345836270074454 + 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.mcap + files: + - path: bag_with_topics_and_service_events.mcap + starting_time: + nanoseconds_since_epoch: 1699345836270074454 + duration: + nanoseconds: 70653944 + message_count: 10 + custom_data: ~ + ros_distro: "" \ No newline at end of file diff --git a/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/bag_with_topics_and_service_events.db3 b/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/bag_with_topics_and_service_events.db3 new file mode 100644 index 0000000000..03f1dd9b4d Binary files /dev/null and b/rosbag2_tests/resources/sqlite3/bag_with_topics_and_service_events/bag_with_topics_and_service_events.db3 differ 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_cpp_get_service_info.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp new file mode 100644 index 0000000000..0713095156 --- /dev/null +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_cpp_get_service_info.cpp @@ -0,0 +1,338 @@ +// 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. +// 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/info.hpp" +#include "rosbag2_cpp/writer.hpp" + +#include "rosbag2_test_common/client_manager.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 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 Rosbag2CPPGetServiceInfoTest + : public rosbag2_test_common::ParametrizedTemporaryDirectoryFixture +{ +public: + void SetUp() override + { + auto bag_name = get_test_name() + "_" + GetParam(); + 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. + std::filesystem::remove_all(root_bag_path_); + } + + void TearDown() override + { + std::filesystem::remove_all(root_bag_path_); + } + + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + static void TearDownTestCase() + { + rclcpp::shutdown(); + } + + template + void start_async_spin(T node) + { + node_spinner_future_ = std::async( + std::launch::async, + [node, this]() -> void { + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(node); + while (!exit_from_node_spinner_) { + exec.spin_some(); + } + }); + } + + void stop_spinning() + { + exit_from_node_spinner_ = true; + if (node_spinner_future_.valid()) { + node_spinner_future_.wait(); + } + } + + 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_.generic_string(); + } + + bool wait_for_subscriptions( + const rosbag2_transport::Recorder & recorder, + const std::vector && topic_names, + std::chrono::duration timeout = std::chrono::seconds(5)) + { + 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(25)); + } + return ready; + } + + // relative path to the root of the bag file. + std::filesystem::path root_bag_path_; + std::future node_spinner_future_; + std::atomic_bool exit_from_node_spinner_{false}; +}; + +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(); + { + // 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_str; + writer.open(storage_options); + test_msgs::msg::BasicTypes msg; + writer.write(msg, "test_topic", rclcpp::Time{}); + } + + rosbag2_cpp::Info info; + 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_infos.empty()); +} + +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(); + + auto service_client_manager = + std::make_shared>( + "test_service"); + + 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(); + + start_async_spin(recorder); + auto cleanup_process_handle = rcpputils::make_scope_exit( + [&]() {stop_spinning();}); + + ASSERT_TRUE(service_client_manager->wait_for_srvice_to_be_ready()); + ASSERT_TRUE(wait_for_subscriptions(*recorder, {"/test_service/_service_event"})); + + 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)); + } + + 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_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"); +} + +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(); + + // Prepare service/client + auto service_client_manager1 = + std::make_shared>( + "test_service1"); + auto service_client_manager2 = + std::make_shared>( + "test_service2"); + + 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); + + 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( + [&]() {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(); + + 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_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_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"); + } +} + +INSTANTIATE_TEST_SUITE_P( + TestInfoGetServiceInfo, + Rosbag2CPPGetServiceInfoTest, + ::testing::ValuesIn(rosbag2_test_common::kTestedStorageIDs) +); 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..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,90 @@ 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( + "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 .*:30:36\\..* \\(1699345836\\..*\\)" + "\nEnd: Nov 7 2023 .*: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(); diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index 50621877f8..840debaa32 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -30,13 +30,17 @@ 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::vector exclude_topics; + std::vector exclude_service_events; // service event topic std::string rmw_serialization_format; std::chrono::milliseconds topic_polling_interval{100}; std::string regex = ""; - std::string exclude = ""; + std::string exclude_regex = ""; std::string node_prefix = ""; std::string compression_mode = ""; std::string compression_format = ""; 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/record_options.cpp b/rosbag2_transport/src/rosbag2_transport/record_options.cpp index ec6a0a9813..d9a7e4b1ae 100644 --- a/rosbag2_transport/src/rosbag2_transport/record_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/record_options.cpp @@ -27,13 +27,17 @@ 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_regex"] = record_options.exclude_regex; + node["exclude_topics"] = record_options.exclude_topics; + 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; @@ -50,9 +54,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 +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", record_options.exclude); + 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_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/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 86b4f7e43e..8c09941b39 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" @@ -185,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() @@ -476,6 +495,7 @@ void RecorderImpl::subscribe_topic(const rosbag2_storage::TopicMetadata & topic) writer_->create_topic(topic); rosbag2_storage::Rosbag2QoS subscription_qos{subscription_qos_for_topic(topic.name)}; + 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..cfb097ad96 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" @@ -117,6 +118,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 +137,91 @@ 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_.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; + } + } + } - 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; - } + if (!record_options_.exclude_topics.empty() && + topic_in_list(topic_name, record_options_.exclude_topics)) + { + return false; + } - if (!has_single_type(topic_name, topic_types)) { - 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, - "Hidden topics are not recorded. Enable them with --include-hidden-topics"); - 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() && + record_options_.regex.empty()) + { + 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() || + !topic_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_service_events.empty() && + topic_in_list(topic_name, record_options_.exclude_service_events)) + { + 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; + } + } } - 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/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"); diff --git a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp index b2c456affe..84b8188c80 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..da9a34d408 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..585ab0776a 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, {}, {}, {"/rosout"}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -60,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( @@ -69,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); @@ -85,3 +85,87 @@ 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, {}, {}, {"/rosout"}, {}, "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->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. + ASSERT_TRUE(client_manager_1->send_request()); + ASSERT_TRUE(client_manager_2->send_request()); + + auto & writer = recorder->get_writer_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); +} + +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, {}, {}, {"/rosout"}, {}, "rmw_format", 100ms}; + 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->wait_for_srvice_to_be_ready()); + + pub_manager.run_publishers(); + + // By default, only client introspection is enabled. + // For one request, service event topic get 2 messages. + ASSERT_TRUE(client_manager_1->send_request()); + + auto & writer = recorder->get_writer_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_all_ignore_leaf_topics.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp index 977aacfe6d..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 @@ -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..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 @@ -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..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 @@ -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..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, {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..015b1ae09e 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp @@ -21,13 +21,17 @@ 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.exclude_topics = {"exclude_topic1", "exclude_topic2"}; + 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"; - original.exclude = "*"; + original.exclude_regex = "[x]/topic"; original.node_prefix = "prefix"; original.compression_mode = "stream"; original.compression_format = "h264"; @@ -45,9 +49,13 @@ 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(exclude_topics); + 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 a20e48cdf4..45e3a7bf08 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_regex_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_regex = topics_regex_to_exclude; // TODO(karsten1987) Refactor this into publication manager @@ -174,3 +178,224 @@ 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_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]+/(.*)"; + + // matching service + 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->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()); + + 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(expected_messages)); + + 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"; + + // 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_service_events.emplace_back(services_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->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()); + + 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(expected_messages)); + + 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..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, {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..8086787c5e 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_regex = "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..7bb26b1676 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"}} }; }; @@ -51,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); @@ -58,6 +62,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 +77,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 +95,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 +108,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 +119,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 +163,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_regex) { rosbag2_transport::RecordOptions record_options; - record_options.exclude = "/inv.*"; - record_options.all = true; + 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_); @@ -167,38 +204,129 @@ TEST_F(RegexFixture, regex_all_and_exclude) } } -TEST_F(RegexFixture, regex_filter_exclude) +TEST_F(RegexFixture, regex_all_topics_and_exclude_topics) { rosbag2_transport::RecordOptions record_options; - record_options.regex = "/invalid.*"; - record_options.exclude = ".invalidated.*"; - record_options.all = false; + 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_); + + 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_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_service_events = { + "/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_); 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.all_topics = true; + 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_); + + 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_regex) +{ + rosbag2_transport::RecordOptions record_options; + record_options.regex = "/invalid.*"; + 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(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_exclude_topics) +{ + rosbag2_transport::RecordOptions record_options; + record_options.regex = "/invalid.*"; + 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("/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_service_events = {"/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(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) { 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; 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 +337,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 +355,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 +369,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 = "/no_exist_service"; + 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(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