From d9b888f6f19d5c1a4ed561f89ee4c4645016aa85 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 13 Jun 2023 16:23:47 +0800 Subject: [PATCH 1/8] Implement service record phase 1 Signed-off-by: Barry Xu --- docs/message_definition_encoding.md | 14 +- ros2bag/ros2bag/verb/info.py | 28 +++- ros2bag/ros2bag/verb/record.py | 77 +++++++-- rosbag2_cpp/CMakeLists.txt | 9 +- rosbag2_cpp/include/rosbag2_cpp/info.hpp | 14 ++ .../local_message_definition_source.hpp | 6 +- .../include/rosbag2_cpp/service_utils.hpp | 31 ++++ rosbag2_cpp/src/rosbag2_cpp/info.cpp | 129 +++++++++++++- .../local_message_definition_source.cpp | 32 ++-- rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 132 +++++++++++++++ .../rosbag2_cpp/writers/sequential_writer.cpp | 13 +- .../test_local_message_definition_source.cpp | 19 ++- .../test/rosbag2_cpp/test_service_utils.cpp | 75 +++++++++ rosbag2_py/CMakeLists.txt | 12 +- rosbag2_py/src/rosbag2_py/_info.cpp | 31 +++- rosbag2_py/src/rosbag2_py/_transport.cpp | 7 +- .../src/rosbag2_py/format_bag_metadata.cpp | 119 ++++++++++++- .../src/rosbag2_py/format_bag_metadata.hpp | 3 +- .../src/rosbag2_py/format_service_info.cpp | 49 ++++++ .../src/rosbag2_py/format_service_info.hpp | 27 +++ rosbag2_py/test/test_reindexer.py | 2 +- rosbag2_py/test/test_transport.py | 4 +- .../service_client_manager.hpp | 125 ++++++++++++++ rosbag2_test_msgdefs/CMakeLists.txt | 1 + rosbag2_test_msgdefs/srv/ComplexSrv.srv | 3 + .../rosbag2_transport/record_options.hpp | 7 +- .../src/rosbag2_transport/record_options.cpp | 14 +- .../src/rosbag2_transport/recorder.cpp | 8 + .../src/rosbag2_transport/topic_filter.cpp | 100 ++++++++--- .../test_keyboard_controls.cpp | 3 +- .../test/rosbag2_transport/test_record.cpp | 19 ++- .../rosbag2_transport/test_record_all.cpp | 85 +++++++++- .../test_record_all_ignore_leaf_topics.cpp | 3 +- ..._record_all_include_unpublished_topics.cpp | 9 +- .../test_record_all_no_discovery.cpp | 3 +- .../test_record_all_use_sim_time.cpp | 2 +- .../rosbag2_transport/test_record_options.cpp | 11 +- .../rosbag2_transport/test_record_regex.cpp | 78 ++++++++- .../test_record_services.cpp | 2 +- .../test/rosbag2_transport/test_rewrite.cpp | 14 +- .../rosbag2_transport/test_topic_filter.cpp | 158 +++++++++++++++--- rosbag2_transport/test/srv/SimpleTest.srv | 3 + 42 files changed, 1353 insertions(+), 128 deletions(-) create mode 100644 rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp create mode 100644 rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp create mode 100644 rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp create mode 100644 rosbag2_py/src/rosbag2_py/format_service_info.cpp create mode 100644 rosbag2_py/src/rosbag2_py/format_service_info.hpp create mode 100644 rosbag2_test_common/include/rosbag2_test_common/service_client_manager.hpp create mode 100644 rosbag2_test_msgdefs/srv/ComplexSrv.srv create mode 100644 rosbag2_transport/test/srv/SimpleTest.srv diff --git a/docs/message_definition_encoding.md b/docs/message_definition_encoding.md index 17e48fdb30..2c9d5db6ff 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/About-ROS-Interfaces.html#message-description-specification) and [.srv](https://docs.ros.org/en/rolling/Concepts/Basic/About-Interfaces.html#services) format, concatenated together in human-readable form with a delimiter. The top-level message definition is present first, with no delimiter. All dependent .msg definitions are preceded by a two-line delimiter: @@ -38,6 +38,18 @@ MSG: my_msgs/msg/BasicMsg float32 my_float ``` +Another example is a service message definition for `my_msgs/srv/ExampleSrv` in `ros2msg` form +``` +# defines a service message that includes a field of a custom message type +my_msgs/BasicMsg request +--- +my_msgs/BasicMsg response +================================================================================ +MSG: my_msgs/msg/BasicMsg +# defines a message with a primitive type field +float32 my_float +``` + ## `ros2idl` encoding The IDL definition of the type specified by name along with all dependent types are stored together. The IDL definitions can be stored in any order. Every definition is preceded by a two-line delimiter: diff --git a/ros2bag/ros2bag/verb/info.py b/ros2bag/ros2bag/verb/info.py index 7b78730edd..5ab1000da0 100644 --- a/ros2bag/ros2bag/verb/info.py +++ b/ros2bag/ros2bag/verb/info.py @@ -26,11 +26,35 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '-t', '--topic-name', action='store_true', help='Only display topic names.' ) + parser.add_argument( + '-v', '--verbose', action='store_true', + help='Display request/response information for services' + ) + + def _is_service_event_topic(self, topic_name, topic_type) -> bool: + + service_event_type_middle = '/srv/' + service_event_type_postfix = '_Event' + + if (service_event_type_middle not in topic_type + or not topic_type.endswith(service_event_type_postfix)): + return False + + service_event_topic_postfix = '/_service_event' + if not topic_name.endswith(service_event_topic_postfix): + return False + + return True def main(self, *, args): # noqa: D102 m = Info().read_metadata(args.bag_path, args.storage) if args.topic_name: for topic_info in m.topics_with_message_count: - print(topic_info.topic_metadata.name) + if not self._is_service_event_topic(topic_info.topic_metadata.name, + topic_info.topic_metadata.type): + print(topic_info.topic_metadata.name) else: - print(m) + if args.verbose: + Info().read_metadata_and_output_service_verbose(args.bag_path, args.storage) + else: + print(m) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 8c49c4b679..dce1361dc7 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -65,15 +65,31 @@ def add_arguments(self, parser, cli_name): # noqa: D102 'topics', nargs='*', default=None, help='List of topics to record.') parser.add_argument( '-a', '--all', action='store_true', - help='Record all topics. Required if no explicit topic list or regex filters.') + help='Record all topics and services (Exclude hidden topic).') + parser.add_argument( + '--all-topics', action='store_true', + help='Record all topics (Exclude hidden topic).') + parser.add_argument( + '--all-services', action='store_true', + help='Record all services via service event topics.') parser.add_argument( '-e', '--regex', default='', - help='Record only topics containing provided regular expression. ' - 'Overrides --all, applies on top of topics list.') + help='Record only topics and services containing provided regular expression. ' + 'Overrides --all, --all-topics and --all-services, applies on top of ' + 'topics list and service list.') parser.add_argument( - '-x', '--exclude', default='', + '--exclude-topics', default='', help='Exclude topics containing provided regular expression. ' - 'Works on top of --all, --regex, or topics list.') + 'Works on top of --all, --all-topics, or --regex.') + parser.add_argument( + '--exclude-services', default='', + help='Exclude services containing provided regular expression. ' + 'Works on top of --all, --all-services, or --regex.') + + # Enable to record service + parser.add_argument( + '--services', type=str, metavar='ServiceName', nargs='+', + help='List of services to record.') # Discovery behavior parser.add_argument( @@ -168,19 +184,34 @@ def add_arguments(self, parser, cli_name): # noqa: D102 'Has no effect if no compression mode is chosen. Default: %(default)s.') 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') + # 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 print_error('Must specify only one option out of --all, --all-topics, ' + '--all-services, --services, topics and --regex') + + # Only one option out of --all, --all-services --services or --regex can be used + if (args.all and args.all_services) or \ + ((args.all or args.all_services) and args.regex) or \ + ((args.all or args.all_services or args.regex) and args.services): + return print_error('Must specify only one option out of --all, --all-services, ' + '--services or --regex') - if args.topics and args.exclude: - return print_error('--exclude argument cannot be used when specifying a list ' - 'of topics explicitly') + # Only one option out of --all, --all-topics, topics or --regex can be used + if (args.all and args.all_topics) or \ + ((args.all or args.all_topics) and args.regex) or \ + ((args.all or args.all_topics or args.regex) and args.topics): + return print_error('Must specify only one option out of --all, --all-topics, ' + 'topics or --regex') - if args.exclude and not(args.regex or args.all): - return print_error('--exclude argument requires either --all or --regex') + if args.exclude_topics and not (args.regex or args.all or args.all_topics): + return print_error('--exclude-topics argument requires either --all, --all-topics ' + 'or --regex') + + if args.exclude_services and not (args.regex or args.all or args.all_services): + return print_error('--exclude-services argument requires either --all, --all-services ' + 'or --regex') uri = args.output or datetime.datetime.now().strftime('rosbag2_%Y_%m_%d-%H_%M_%S') @@ -232,14 +263,15 @@ def main(self, *, args): # noqa: D102 custom_data=custom_data ) record_options = RecordOptions() - record_options.all = args.all + record_options.all_topics = args.all_topics or args.all record_options.is_discovery_disabled = args.no_discovery record_options.topics = args.topics record_options.rmw_serialization_format = args.serialization_format record_options.topic_polling_interval = datetime.timedelta( milliseconds=args.polling_interval) record_options.regex = args.regex - record_options.exclude = args.exclude + record_options.exclude_topics = args.exclude_topics + record_options.exclude_services = args.exclude_services record_options.node_prefix = NODE_NAME_PREFIX record_options.compression_mode = args.compression_mode record_options.compression_format = args.compression_format @@ -251,6 +283,15 @@ 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 + services = [] + if args.services and len(args.services) != 0: + for s in args.services: + name = '/' + s if s[0] != '/' else s + services.append(name + '/_service_event') + record_options.services = services recorder = Recorder() diff --git a/rosbag2_cpp/CMakeLists.txt b/rosbag2_cpp/CMakeLists.txt index 8d61f3af6d..8ad64fc2fe 100644 --- a/rosbag2_cpp/CMakeLists.txt +++ b/rosbag2_cpp/CMakeLists.txt @@ -72,7 +72,8 @@ add_library(${PROJECT_NAME} SHARED src/rosbag2_cpp/types/introspection_message.cpp src/rosbag2_cpp/writer.cpp src/rosbag2_cpp/writers/sequential_writer.cpp - src/rosbag2_cpp/reindexer.cpp) + src/rosbag2_cpp/reindexer.cpp + src/rosbag2_cpp/service_utils.cpp) target_link_libraries(${PROJECT_NAME} PUBLIC @@ -257,6 +258,12 @@ if(BUILD_TESTING) if(TARGET test_time_controller_clock) target_link_libraries(test_time_controller_clock ${PROJECT_NAME}) endif() + + ament_add_gmock(test_service_utils + test/rosbag2_cpp/test_service_utils.cpp) + if(TARGET test_service_utils) + target_link_libraries(test_service_utils ${PROJECT_NAME}) + endif() endif() ament_package() diff --git a/rosbag2_cpp/include/rosbag2_cpp/info.hpp b/rosbag2_cpp/include/rosbag2_cpp/info.hpp index c3c2b2b7d7..3d109d4b38 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 struct +{ + std::string name; + std::string type; + std::string serialization_format; + size_t request_count; + size_t response_count; +} service_info; + 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..f6ddce86a2 --- /dev/null +++ b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp @@ -0,0 +1,31 @@ +// 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 + +namespace rosbag2_cpp +{ +bool is_service_event_topic(const std::string & topic, const std::string & topic_type); + +std::string service_event_topic_name_to_service_name(const std::string & topic_name); + +std::string service_event_topic_type_to_service_type(const std::string & topic_type); + +size_t get_serialization_size_for_service_metadata_event(); +} + +#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..c91a8a4a99 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -12,18 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rosbag2_cpp/info.hpp" - -#include +#include +#include +#include #include #include +#include "rcl/service_introspection.h" +#include "rmw/rmw.h" + #include "rcpputils/filesystem_helper.hpp" +#include "rosbag2_cpp/info.hpp" +#include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/logging.hpp" #include "rosbag2_storage/metadata_io.hpp" #include "rosbag2_storage/storage_interfaces/read_only_interface.hpp" #include "rosbag2_storage/storage_factory.hpp" +#include "service_msgs/msg/service_event_info.hpp" + namespace rosbag2_cpp { @@ -52,4 +59,120 @@ rosbag2_storage::BagMetadata Info::read_metadata( return storage->get_metadata(); } +namespace +{ +struct client_id_hash +{ + std::size_t operator()(const std::array & client_id) const + { + std::hash hasher; + std::size_t seed = 0; + for (const auto & value : client_id) { + // 0x9e3779b9 is from https://cryptography.fandom.com/wiki/Tiny_Encryption_Algorithm + seed ^= hasher(value) + 0x9e3779b9 + (seed << 6) + (seed >> 2); + } + return seed; + } +}; + +using client_id = std::array; +using sequence_set = std::unordered_set; +struct service_req_resp_info +{ + std::unordered_map request; + std::unordered_map response; +}; +} // namespace + +std::vector> Info::read_service_info( + const std::string & uri, const std::string & storage_id) +{ + rosbag2_storage::StorageFactory factory; + auto storage = factory.open_read_only({uri, storage_id}); + if (!storage) { + throw std::runtime_error("No plugin detected that could open file " + uri); + } + + using service_analysis = + std::unordered_map>; + + std::unordered_map> all_service_info; + service_analysis service_process_info; + + auto all_topics_types = storage->get_all_topics_and_types(); + for (auto & t : all_topics_types) { + if (is_service_event_topic(t.name, t.type)) { + auto service_info = std::make_shared(); + service_info->name = service_event_topic_name_to_service_name(t.name); + service_info->type = service_event_topic_type_to_service_type(t.type); + service_info->serialization_format = t.serialization_format; + all_service_info.emplace(t.name, service_info); + service_process_info[t.name] = std::make_shared(); + } + } + + std::vector> ret_service_info; + + if (!all_service_info.empty()) { + auto msg = service_msgs::msg::ServiceEventInfo(); + const rosidl_message_type_support_t * type_support_info = + rosidl_typesupport_cpp:: + get_message_type_support_handle(); + + while (storage->has_next()) { + auto bag_msg = storage->read_next(); + + // Check if topic is service event topic + auto one_service_info = all_service_info.find(bag_msg->topic_name); + if (one_service_info == all_service_info.end()) { + continue; + } + + auto ret = rmw_deserialize( + bag_msg->serialized_data.get(), + type_support_info, + reinterpret_cast(&msg)); + if (ret != RMW_RET_OK) { + throw std::runtime_error( + "It failed to deserialize message from " + bag_msg->topic_name + " !"); + } + + switch (msg.event_type) { + case service_msgs::msg::ServiceEventInfo::REQUEST_SENT: + case service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED: + service_process_info[bag_msg->topic_name] + ->request[msg.client_gid].emplace(msg.sequence_number); + break; + case service_msgs::msg::ServiceEventInfo::RESPONSE_SENT: + case service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED: + service_process_info[bag_msg->topic_name] + ->response[msg.client_gid].emplace(msg.sequence_number); + break; + } + } + + for (auto & [topic_name, service_info] : service_process_info) { + size_t count = 0; + // Get the number of request from all clients + for (auto &[client_id, request_list] : service_info->request) { + count += request_list.size(); + } + all_service_info[topic_name]->request_count = count; + + count = 0; + // Get the number of response from all clients + for (auto &[client_id, response_list] : service_info->response) { + count += response_list.size(); + } + all_service_info[topic_name]->response_count = count; + } + + for (auto & [topic_name, service_info] : all_service_info) { + ret_service_info.emplace_back(std::move(service_info)); + } + } + + return ret_service_info; +} + } // namespace rosbag2_cpp diff --git a/rosbag2_cpp/src/rosbag2_cpp/message_definitions/local_message_definition_source.cpp b/rosbag2_cpp/src/rosbag2_cpp/message_definitions/local_message_definition_source.cpp index 74e04f9f01..1d8e5c5db5 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/message_definitions/local_message_definition_source.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/message_definitions/local_message_definition_source.cpp @@ -48,9 +48,9 @@ class TypenameNotUnderstoodError : public std::exception }; // Match datatype names (foo_msgs/Bar or foo_msgs/msg/Bar) -static const std::regex PACKAGE_TYPENAME_REGEX{R"(^([a-zA-Z0-9_]+)/(?:msg/)?([a-zA-Z0-9_]+)$)"}; +static const std::regex PACKAGE_TYPENAME_REGEX{R"(^([a-zA-Z0-9_]+)/(?:msg/|srv/)?([a-zA-Z0-9_]+)$)"}; -// Match field types from .msg definitions ("foo_msgs/Bar" in "foo_msgs/Bar[] bar") +// Match field types from .msg and .srv definitions ("foo_msgs/Bar" in "foo_msgs/Bar[] bar") static const std::regex MSG_FIELD_TYPE_REGEX{R"((?:^|\n)\s*([a-zA-Z0-9_/]+)(?:\[[^\]]*\])?\s+)"}; // match field types from `.idl` definitions ("foo_msgs/msg/bar" in #include ) @@ -102,6 +102,7 @@ std::set parse_definition_dependencies( { switch (format) { case LocalMessageDefinitionSource::Format::MSG: + case LocalMessageDefinitionSource::Format::SRV: return parse_msg_dependencies(text, package_context); case LocalMessageDefinitionSource::Format::IDL: return parse_idl_dependencies(text); @@ -117,6 +118,8 @@ static const char * extension_for_format(LocalMessageDefinitionSource::Format fo return ".msg"; case LocalMessageDefinitionSource::Format::IDL: return ".idl"; + case LocalMessageDefinitionSource::Format::SRV: + return ".srv"; default: throw std::runtime_error("switch is not exhaustive"); } @@ -134,6 +137,9 @@ std::string LocalMessageDefinitionSource::delimiter( case Format::IDL: result += "IDL: "; break; + case Format::SRV: + result += "SRV: "; + break; default: throw std::runtime_error("switch is not exhaustive"); } @@ -166,7 +172,9 @@ const LocalMessageDefinitionSource::MessageSpec & LocalMessageDefinitionSource:: } std::string package = match[1]; std::string share_dir = ament_index_cpp::get_package_share_directory(package); - std::ifstream file{share_dir + "/msg/" + match[2].str() + + std::string dir = definition_identifier.format() == Format::MSG || + definition_identifier.format() == Format::IDL ? "/msg/" : "/srv/"; + std::ifstream file{share_dir + dir + match[2].str() + extension_for_format(definition_identifier.format())}; if (!file.good()) { throw DefinitionNotFoundError(definition_identifier.topic_type()); @@ -183,7 +191,7 @@ const LocalMessageDefinitionSource::MessageSpec & LocalMessageDefinitionSource:: } rosbag2_storage::MessageDefinition LocalMessageDefinitionSource::get_full_text( - const std::string & root_topic_type) + const std::string & root_type) { std::unordered_set seen_deps; @@ -191,12 +199,14 @@ rosbag2_storage::MessageDefinition LocalMessageDefinitionSource::get_full_text( [&](const DefinitionIdentifier & definition_identifier, int32_t depth) { if (depth <= 0) { throw std::runtime_error{ - "Reached max recursion depth resolving definition of " + root_topic_type}; + "Reached max recursion depth resolving definition of " + root_type}; } const MessageSpec & spec = load_message_spec(definition_identifier); std::string result = spec.text; for (const auto & dep_name : spec.dependencies) { - DefinitionIdentifier dep(dep_name, definition_identifier.format()); + Format format = definition_identifier.format() == Format::SRV ? + Format::MSG : definition_identifier.format(); + DefinitionIdentifier dep(dep_name, format); bool inserted = seen_deps.insert(dep).second; if (inserted) { result += "\n"; @@ -208,14 +218,14 @@ rosbag2_storage::MessageDefinition LocalMessageDefinitionSource::get_full_text( }; std::string result; - Format format = Format::MSG; + Format format = root_type.find("/srv/") != std::string::npos ? Format::SRV : Format::MSG; int32_t max_recursion_depth = ROSBAG2_CPP_LOCAL_MESSAGE_DEFINITION_SOURCE_MAX_RECURSION_DEPTH; try { - result = append_recursive(DefinitionIdentifier(root_topic_type, format), max_recursion_depth); + result = append_recursive(DefinitionIdentifier(root_type, format), max_recursion_depth); } catch (const DefinitionNotFoundError & err) { ROSBAG2_CPP_LOG_WARN("No .msg definition for %s, falling back to IDL", err.what()); format = Format::IDL; - DefinitionIdentifier root_definition_identifier(root_topic_type, format); + DefinitionIdentifier root_definition_identifier(root_type, format); result = (delimiter(root_definition_identifier) + append_recursive(root_definition_identifier, max_recursion_depth)); } catch (const TypenameNotUnderstoodError & err) { @@ -230,6 +240,7 @@ rosbag2_storage::MessageDefinition LocalMessageDefinitionSource::get_full_text( out.encoding = "unknown"; break; case Format::MSG: + case Format::SRV: out.encoding = "ros2msg"; break; case Format::IDL: @@ -238,8 +249,9 @@ rosbag2_storage::MessageDefinition LocalMessageDefinitionSource::get_full_text( default: throw std::runtime_error("switch is not exhaustive"); } + out.encoded_message_definition = result; - out.topic_type = root_topic_type; + out.topic_type = root_type; return out; } } // namespace rosbag2_cpp diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp new file mode 100644 index 0000000000..e658db7b97 --- /dev/null +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -0,0 +1,132 @@ +// Copyright 2023 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/service_introspection.h" + +#include "rosbag2_cpp/service_utils.hpp" +#include "rosidl_typesupport_cpp/message_type_support_dispatch.hpp" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" + +#include "service_msgs/msg/service_event_info.hpp" + +namespace rosbag2_cpp +{ +const char * service_event_topic_type_postfix = "_Event"; +const char * service_event_topic_type_middle = "/srv/"; + +bool is_service_event_topic(const std::string & topic, const std::string & topic_type) +{ + if (topic.length() <= (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)) { + return false; + } + + std::string end_topic_name = topic.substr( + topic.length() - (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)); + + if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { + return false; + } + + // Should include '/srv/' in type + if (topic_type.find(service_event_topic_type_middle) == std::string::npos) { + return false; + } + + std::string end_topic_type = topic_type.substr( + topic_type.length() - std::strlen(service_event_topic_type_postfix)); + + return end_topic_name == RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX && + end_topic_type == service_event_topic_type_postfix; +} + +std::string service_event_topic_name_to_service_name(const std::string & topic_name) +{ + if (topic_name.length() <= (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)) { + return std::string(); + } + + if (topic_name.substr( + topic_name.length() - + (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)) != + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + { + return std::string(); + } + + std::string service_name = topic_name.substr( + 0, topic_name.length() - (sizeof(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - 1)); + + return service_name; +} + +std::string service_event_topic_type_to_service_type(const std::string & topic_type) +{ + if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { + return std::string(); + } + + // Should include '/srv/' in type + if (topic_type.find(service_event_topic_type_middle) == std::string::npos) { + return std::string(); + } + + if (topic_type.substr(topic_type.length() - std::strlen(service_event_topic_type_postfix)) != + service_event_topic_type_postfix) + { + return std::string(); + } + + std::string service_type = topic_type.substr( + 0, topic_type.length() - strlen(service_event_topic_type_postfix)); + + return service_type; +} + +size_t get_serialization_size_for_service_metadata_event() +{ + // Since the size is fixed, it only needs to be calculated once. + static size_t size = 0; + + if (size != 0) { + return size; + } + + const rosidl_message_type_support_t * type_support_info = + rosidl_typesupport_cpp:: + get_message_type_support_handle(); + + // Get the serialized size of service event info + const rosidl_message_type_support_t * type_support_handle = + rosidl_typesupport_cpp::get_message_typesupport_handle_function( + type_support_info, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (type_support_handle == nullptr) { + std::runtime_error("Cannot get ServiceEventInfo typesupport handle !"); + } + + auto service_event_info = + static_cast( + type_support_handle->data); + + // endian type (4 size) + service event info size + empty request (4 bytes) + // + emtpy response (4 bytes) + size = 4 + service_event_info->size_of_ + 4 + 4; + + return size; +} + +} // namespace rosbag2_cpp diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index d32951e6f7..77d7b04618 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,17 @@ 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 = topic_with_type.type.substr( + 0, + topic_with_type.type.length() - strlen("_Event")); + } else { + topic_type = topic_with_type.type; + } + try { definition = message_definitions_.get_full_text(topic_type); } catch (DefinitionNotFoundError &) { diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_local_message_definition_source.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_local_message_definition_source.cpp index 7e7d09ae1d..526847dee6 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_local_message_definition_source.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_local_message_definition_source.cpp @@ -62,6 +62,23 @@ TEST(test_local_message_definition_source, can_find_msg_deps) "float32 c\n"); } +TEST(test_local_message_definition_source, can_find_srv_deps) +{ + LocalMessageDefinitionSource source; + auto result = source.get_full_text("rosbag2_test_msgdefs/srv/ComplexSrv"); + std::cout << result.encoded_message_definition << std::endl; + ASSERT_EQ(result.encoding, "ros2msg"); + ASSERT_EQ( + result.encoded_message_definition, + "rosbag2_test_msgdefs/BasicMsg req\n" + "---\n" + "rosbag2_test_msgdefs/BasicMsg resp\n" + "\n" + "================================================================================\n" + "MSG: rosbag2_test_msgdefs/BasicMsg\n" + "float32 c\n"); +} + TEST(test_local_message_definition_source, can_find_idl_deps) { LocalMessageDefinitionSource source; @@ -132,7 +149,7 @@ TEST(test_local_message_definition_source, no_crash_on_bad_name) rosbag2_storage::MessageDefinition result; ASSERT_NO_THROW( { - result = source.get_full_text("rosbag2_test_msgdefs/srv/BasicSrv_Request"); + result = source.get_full_text("rosbag2_test_msgdefs/idl/BasicSrv_Request"); }); ASSERT_EQ(result.encoding, "unknown"); } diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp new file mode 100644 index 0000000000..b04c0e6afd --- /dev/null +++ b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp @@ -0,0 +1,75 @@ +// Copyright 2023 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include + +#include +#include + +#include "rosbag2_cpp/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(test_data.first.first, test_data.first.second) == + test_data.second); + } +} + +TEST_F(ServiceUtilsTest, check_service_event_topic_name_to_service_name) +{ + std::vector> all_test_data = + { + {"/abc/_service_event", "/abc"}, + {"/_service_event", ""}, + {"/abc/service_event", ""} + }; + + for (const auto & test_data : all_test_data) { + EXPECT_TRUE( + rosbag2_cpp::service_event_topic_name_to_service_name(test_data.first) == test_data.second); + } +} + +TEST_F(ServiceUtilsTest, check_service_event_topic_type_to_service_type) +{ + std::vector> all_test_data = + { + {"package/srv/xyz_Event", "package/srv/xyz"}, + {"package/xyz_Event", ""}, + {"package/srv/Event", ""} + }; + + for (const auto & test_data : all_test_data) { + EXPECT_EQ( + rosbag2_cpp::service_event_topic_type_to_service_type(test_data.first), + test_data.second + ); + } +} diff --git a/rosbag2_py/CMakeLists.txt b/rosbag2_py/CMakeLists.txt index 0b71864e54..80bc823043 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -48,6 +48,14 @@ ament_target_dependencies(_compression_options PUBLIC "rosbag2_compression" ) +add_library(_format_output SHARED + src/rosbag2_py/format_bag_metadata.cpp + src/rosbag2_py/format_service_info.cpp +) +target_link_libraries(_format_output PUBLIC + rosbag2_cpp::rosbag2_cpp +) + pybind11_add_module(_reader SHARED src/rosbag2_py/_reader.cpp ) @@ -59,11 +67,11 @@ target_link_libraries(_reader PUBLIC pybind11_add_module(_storage SHARED src/rosbag2_py/_storage.cpp - src/rosbag2_py/format_bag_metadata.cpp ) target_link_libraries(_storage PUBLIC rosbag2_cpp::rosbag2_cpp rosbag2_storage::rosbag2_storage + _format_output ) pybind11_add_module(_writer SHARED @@ -81,6 +89,7 @@ pybind11_add_module(_info SHARED target_link_libraries(_info PUBLIC rosbag2_cpp::rosbag2_cpp rosbag2_storage::rosbag2_storage + _format_output ) pybind11_add_module(_transport SHARED @@ -105,6 +114,7 @@ target_link_libraries(_reindexer PUBLIC install( TARGETS _compression_options + _format_output _reader _storage _writer diff --git a/rosbag2_py/src/rosbag2_py/_info.cpp b/rosbag2_py/src/rosbag2_py/_info.cpp index fdd7d00dc8..061e7317c3 100644 --- a/rosbag2_py/src/rosbag2_py/_info.cpp +++ b/rosbag2_py/src/rosbag2_py/_info.cpp @@ -15,6 +15,8 @@ #include #include +#include "format_bag_metadata.hpp" +#include "format_service_info.hpp" #include "rosbag2_cpp/info.hpp" #include "rosbag2_storage/bag_metadata.hpp" @@ -38,6 +40,30 @@ class Info return info_->read_metadata(uri, storage_id); } + void read_metadata_and_output_service_verbose( + const std::string & uri, + const std::string & storage_id) + { + auto metadata_info = read_metadata(uri, storage_id); + + std::vector> all_services_info; + for (auto & file_info : metadata_info.files) { + auto services_info = info_->read_service_info( + uri + "/" + file_info.path, + metadata_info.storage_identifier); + if (!services_info.empty()) { + all_services_info.insert( + all_services_info.end(), + services_info.begin(), + services_info.end()); + } + } + + // Output formatted metadata and service info + std::cout << format_bag_meta_data(metadata_info, true); + std::cout << format_service_info(all_services_info) << std::endl; + } + protected: std::unique_ptr info_; }; @@ -49,5 +75,8 @@ PYBIND11_MODULE(_info, m) { pybind11::class_(m, "Info") .def(pybind11::init()) - .def("read_metadata", &rosbag2_py::Info::read_metadata); + .def("read_metadata", &rosbag2_py::Info::read_metadata) + .def( + "read_metadata_and_output_service_verbose", + &rosbag2_py::Info::read_metadata_and_output_service_verbose); } diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index 0237bf5cfa..d9ee4c3833 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -353,13 +353,14 @@ PYBIND11_MODULE(_transport, m) { py::class_(m, "RecordOptions") .def(py::init<>()) - .def_readwrite("all", &RecordOptions::all) + .def_readwrite("all_topics", &RecordOptions::all_topics) .def_readwrite("is_discovery_disabled", &RecordOptions::is_discovery_disabled) .def_readwrite("topics", &RecordOptions::topics) .def_readwrite("rmw_serialization_format", &RecordOptions::rmw_serialization_format) .def_readwrite("topic_polling_interval", &RecordOptions::topic_polling_interval) .def_readwrite("regex", &RecordOptions::regex) - .def_readwrite("exclude", &RecordOptions::exclude) + .def_readwrite("exclude_topics", &RecordOptions::exclude_topics) + .def_readwrite("exclude_services", &RecordOptions::exclude_services) .def_readwrite("node_prefix", &RecordOptions::node_prefix) .def_readwrite("compression_mode", &RecordOptions::compression_mode) .def_readwrite("compression_format", &RecordOptions::compression_format) @@ -374,6 +375,8 @@ PYBIND11_MODULE(_transport, m) { .def_readwrite("start_paused", &RecordOptions::start_paused) .def_readwrite("ignore_leaf_topics", &RecordOptions::ignore_leaf_topics) .def_readwrite("use_sim_time", &RecordOptions::use_sim_time) + .def_readwrite("services", &RecordOptions::services) + .def_readwrite("all_services", &RecordOptions::all_services) ; py::class_(m, "Player") diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp index fc8408db45..a2189bb1fa 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp @@ -12,11 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "format_bag_metadata.hpp" - #include #include #include +#include #include #include #include @@ -26,8 +25,11 @@ #include #endif +#include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/bag_metadata.hpp" +#include "format_bag_metadata.hpp" + namespace { @@ -124,17 +126,105 @@ void format_topics_with_type( info_stream << std::endl; }; - print_topic_info(topics[0]); size_t number_of_topics = topics.size(); - for (size_t j = 1; j < number_of_topics; ++j) { + size_t i = 0; + // Find first topic which isn't service event topic + while (i < number_of_topics && + rosbag2_cpp::is_service_event_topic( + topics[i].topic_metadata.name, + topics[i].topic_metadata.type)) + { + i++; + } + + if (i == number_of_topics) { + return; + } + + print_topic_info(topics[i]); + for (size_t j = ++i; j < number_of_topics; ++j) { + if (rosbag2_cpp::is_service_event_topic( + topics[j].topic_metadata.name, topics[j].topic_metadata.type)) + { + continue; + } indent(info_stream, indentation_spaces); print_topic_info(topics[j]); } } +struct ServiceMetadata +{ + std::string name; + std::string type; + std::string serialization_format; +}; + +struct ServiceInformation +{ + ServiceMetadata service_metadata; + size_t event_message_count; +}; + +std::vector> filter_service_event_topic( + const std::vector & topics_with_message_count, + size_t & total_service_event_msg_count) +{ + total_service_event_msg_count = 0; + std::vector> service_info_list; + + for (auto & topic : topics_with_message_count) { + if (rosbag2_cpp::is_service_event_topic( + topic.topic_metadata.name, topic.topic_metadata.type)) + { + auto service_info = std::make_shared(); + service_info->service_metadata.name = + rosbag2_cpp::service_event_topic_name_to_service_name(topic.topic_metadata.name); + service_info->service_metadata.type = + rosbag2_cpp::service_event_topic_type_to_service_type(topic.topic_metadata.type); + service_info->service_metadata.serialization_format = + topic.topic_metadata.serialization_format; + service_info->event_message_count = topic.message_count; + total_service_event_msg_count += topic.message_count; + service_info_list.emplace_back(service_info); + } + } + + return service_info_list; +} + +void format_service_with_type( + const std::vector> & services, + std::stringstream & info_stream, + int indentation_spaces) +{ + if (services.empty()) { + info_stream << std::endl; + return; + } + + auto print_service_info = + [&info_stream](const std::shared_ptr & si) -> void { + info_stream << "Service: " << si->service_metadata.name << " | "; + info_stream << "Type: " << si->service_metadata.type << " | "; + info_stream << "Event Count: " << si->event_message_count << " | "; + info_stream << "Serialization Format: " << si->service_metadata.serialization_format; + info_stream << std::endl; + }; + + print_service_info(services[0]); + auto number_of_services = services.size(); + for (size_t j = 1; j < number_of_services; ++j) { + indent(info_stream, indentation_spaces); + print_service_info(services[j]); + } +} + } // namespace -std::string format_bag_meta_data(const rosbag2_storage::BagMetadata & metadata) +std::string format_bag_meta_data( + const rosbag2_storage::BagMetadata & metadata, + bool only_topic) { auto start_time = metadata.starting_time.time_since_epoch(); auto end_time = start_time + metadata.duration; @@ -145,6 +235,14 @@ std::string format_bag_meta_data(const rosbag2_storage::BagMetadata & metadata) ros_distro = "unknown"; } + size_t total_service_event_msg_count = 0; + std::vector> service_info_list; + if (!only_topic) { + service_info_list = filter_service_event_topic( + metadata.topics_with_message_count, + total_service_event_msg_count); + } + info_stream << std::endl; info_stream << "Files: "; format_file_paths(metadata.relative_file_paths, info_stream, indentation_spaces); @@ -157,10 +255,19 @@ std::string format_bag_meta_data(const rosbag2_storage::BagMetadata & metadata) info_stream << "Start: " << format_time_point(start_time) << std::endl; info_stream << "End: " << format_time_point(end_time) << std::endl; - info_stream << "Messages: " << metadata.message_count << std::endl; + info_stream << "Messages: " << metadata.message_count - total_service_event_msg_count << + std::endl; info_stream << "Topic information: "; format_topics_with_type( metadata.topics_with_message_count, info_stream, indentation_spaces); + if (!only_topic) { + info_stream << "Service: " << service_info_list.size() << std::endl; + info_stream << "Service information: "; + if (service_info_list.size() != 0) { + format_service_with_type(service_info_list, info_stream, indentation_spaces + 2); + } + } + return info_stream.str(); } diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp index 30cd8d4344..ba2e868ed7 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp @@ -19,6 +19,7 @@ #include "rosbag2_storage/bag_metadata.hpp" -std::string format_bag_meta_data(const rosbag2_storage::BagMetadata & metadata); +std::string format_bag_meta_data( + const rosbag2_storage::BagMetadata & metadata, bool only_topic = false); #endif // ROSBAG2_PY__FORMAT_BAG_METADATA_HPP_ diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.cpp b/rosbag2_py/src/rosbag2_py/format_service_info.cpp new file mode 100644 index 0000000000..07ee80edec --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/format_service_info.cpp @@ -0,0 +1,49 @@ +// Copyright 2023 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "format_service_info.hpp" + +std::string +format_service_info(std::vector> & service_info_list) +{ + std::stringstream info_stream; + int indentation_spaces = 21; + info_stream << "Service: " << service_info_list.size() << std::endl; + info_stream << "Service information: "; + + if (service_info_list.empty()) { + return info_stream.str(); + } + + auto print_service_info = + [&info_stream](const std::shared_ptr & si) -> void { + info_stream << "Service: " << si->name << " | "; + info_stream << "Type: " << si->type << " | "; + info_stream << "Request Count: " << si->request_count << " | "; + info_stream << "Response Count: " << si->response_count << " | "; + info_stream << "Serialization Format: " << si->serialization_format; + info_stream << std::endl; + }; + + print_service_info(service_info_list[0]); + auto number_of_services = service_info_list.size(); + for (size_t j = 1; j < number_of_services; ++j) { + info_stream << std::string(indentation_spaces, ' '); + print_service_info(service_info_list[j]); + } + + return info_stream.str(); +} diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.hpp b/rosbag2_py/src/rosbag2_py/format_service_info.hpp new file mode 100644 index 0000000000..f1aac050a4 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/format_service_info.hpp @@ -0,0 +1,27 @@ +// Copyright 2023 Sony Group Corporation. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROSBAG2_PY__FORMAT_SERVICE_INFO_HPP_ +#define ROSBAG2_PY__FORMAT_SERVICE_INFO_HPP_ + +#include +#include +#include + +#include "rosbag2_cpp/info.hpp" + +std::string format_service_info( + std::vector> & service_info); + +#endif // ROSBAG2_PY__FORMAT_SERVICE_INFO_HPP_ diff --git a/rosbag2_py/test/test_reindexer.py b/rosbag2_py/test/test_reindexer.py index caa9375ffb..5369feb996 100644 --- a/rosbag2_py/test/test_reindexer.py +++ b/rosbag2_py/test/test_reindexer.py @@ -43,7 +43,7 @@ def test_reindexer_multiple_files(storage_id): reindexer = rosbag2_py.Reindexer() reindexer.reindex(storage_options) - assert(result_path.exists()) + assert result_path.exists() try: result_path.unlink() diff --git a/rosbag2_py/test/test_transport.py b/rosbag2_py/test/test_transport.py index 548708d409..fe01d28b93 100644 --- a/rosbag2_py/test/test_transport.py +++ b/rosbag2_py/test/test_transport.py @@ -50,7 +50,7 @@ def test_record_cancel(tmp_path, storage_id): recorder = rosbag2_py.Recorder() record_options = rosbag2_py.RecordOptions() - record_options.all = True + record_options.all_topics = True record_options.is_discovery_disabled = False record_options.topic_polling_interval = datetime.timedelta(milliseconds=100) @@ -83,7 +83,7 @@ def test_record_cancel(tmp_path, storage_id): record_thread.join() metadata = metadata_io.read_metadata(bag_path) - assert(len(metadata.relative_file_paths)) + assert len(metadata.relative_file_paths) storage_path = Path(metadata.relative_file_paths[0]) assert wait_for(lambda: storage_path.is_file(), timeout=rclpy.duration.Duration(seconds=3)) diff --git a/rosbag2_test_common/include/rosbag2_test_common/service_client_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/service_client_manager.hpp new file mode 100644 index 0000000000..2862b1fb98 --- /dev/null +++ b/rosbag2_test_common/include/rosbag2_test_common/service_client_manager.hpp @@ -0,0 +1,125 @@ +// 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_CLIENT_MANAGER_HPP_ +#define ROSBAG2_TEST_COMMON__SERVICE_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 ServiceClientManager : public rclcpp::Node +{ +public: + explicit ServiceClientManager( + std::string service_name, + size_t client_number = 1, + bool service_event_contents = false, + bool client_event_contents = true) + : Node("service_client_manager_" + std::to_string(rclcpp::Clock().now().nanoseconds()), + rclcpp::NodeOptions().start_parameter_services(false).start_parameter_event_publisher( + false).enable_rosout(false)), + service_name_(service_name), + client_number_(client_number), + enable_service_event_contents_(service_event_contents), + enable_client_event_contents_(client_event_contents) + { + auto echo_process = + [this](const std::shared_ptr request_header, + const std::shared_ptr request, + std::shared_ptr response) -> void + { + // Do nothing + (void)request_header; + (void)request; + (void)response; + }; + + service_ = create_service(service_name_, echo_process); + + rcl_service_introspection_state_t introspection_state; + if (enable_service_event_contents_) { + introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; + } else { + introspection_state = RCL_SERVICE_INTROSPECTION_OFF; + } + service_->configure_introspection( + get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state); + + if (enable_client_event_contents_) { + introspection_state = RCL_SERVICE_INTROSPECTION_CONTENTS; + } else { + introspection_state = RCL_SERVICE_INTROSPECTION_OFF; + } + + for (size_t i = 0; i < client_number_; i++) { + auto client = create_client(service_name_); + client->configure_introspection( + get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state); + clients_.emplace_back(client); + } + } + + bool check_service_ready() + { + for (auto & client : clients_) { + if (!client->service_is_ready()) { + return false; + } + } + return true; + } + + bool send_request() + { + if (!check_service_ready()) { + return false; + } + + for (auto & client : clients_) { + auto request = std::make_shared(); + auto result = client->async_send_request(request); + // Wait for the result. + if (rclcpp::executors::spin_node_until_future_complete( + exec_, get_node_base_interface(), result) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_INFO( + rclcpp::get_logger("service_client_manager"), "Failed to get response !"); + return false; + } + } + return true; + } + + using client_shared_ptr = typename rclcpp::Client::SharedPtr; + +private: + rclcpp::executors::SingleThreadedExecutor exec_; + typename rclcpp::Service::SharedPtr service_; + std::vector clients_; + const std::string service_name_; + size_t client_number_; + bool enable_service_event_contents_; + bool enable_client_event_contents_; +}; +} // namespace rosbag2_test_common + +#endif // ROSBAG2_TEST_COMMON__SERVICE_CLIENT_MANAGER_HPP_ diff --git a/rosbag2_test_msgdefs/CMakeLists.txt b/rosbag2_test_msgdefs/CMakeLists.txt index 32199bf446..f50036ab08 100644 --- a/rosbag2_test_msgdefs/CMakeLists.txt +++ b/rosbag2_test_msgdefs/CMakeLists.txt @@ -15,6 +15,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/ComplexMsg.msg" "msg/ComplexMsgDependsOnIdl.msg" "srv/BasicSrv.srv" + "srv/ComplexSrv.srv" ADD_LINTER_TESTS ) diff --git a/rosbag2_test_msgdefs/srv/ComplexSrv.srv b/rosbag2_test_msgdefs/srv/ComplexSrv.srv new file mode 100644 index 0000000000..d987d100fd --- /dev/null +++ b/rosbag2_test_msgdefs/srv/ComplexSrv.srv @@ -0,0 +1,3 @@ +rosbag2_test_msgdefs/BasicMsg req +--- +rosbag2_test_msgdefs/BasicMsg resp diff --git a/rosbag2_transport/include/rosbag2_transport/record_options.hpp b/rosbag2_transport/include/rosbag2_transport/record_options.hpp index a387ab0998..385187133a 100644 --- a/rosbag2_transport/include/rosbag2_transport/record_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/record_options.hpp @@ -30,13 +30,16 @@ namespace rosbag2_transport struct RecordOptions { public: - bool all = false; + bool all_topics = false; + bool all_services = false; bool is_discovery_disabled = false; std::vector topics; + std::vector services; // service event topic std::string rmw_serialization_format; std::chrono::milliseconds topic_polling_interval{100}; std::string regex = ""; - std::string exclude = ""; + std::string exclude_topics = ""; + std::string exclude_services = ""; std::string node_prefix = ""; std::string compression_mode = ""; std::string compression_format = ""; diff --git a/rosbag2_transport/src/rosbag2_transport/record_options.cpp b/rosbag2_transport/src/rosbag2_transport/record_options.cpp index 9f94ac68a3..c8b8519a24 100644 --- a/rosbag2_transport/src/rosbag2_transport/record_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/record_options.cpp @@ -42,13 +42,16 @@ Node convert::encode( const rosbag2_transport::RecordOptions & record_options) { Node node; - node["all"] = record_options.all; + node["all_topics"] = record_options.all_topics; + node["all_services"] = record_options.all_services; node["is_discovery_disabled"] = record_options.is_discovery_disabled; node["topics"] = record_options.topics; + node["services"] = record_options.services; node["rmw_serialization_format"] = record_options.rmw_serialization_format; node["topic_polling_interval"] = record_options.topic_polling_interval; node["regex"] = record_options.regex; - node["exclude"] = record_options.exclude; + node["exclude_topics"] = record_options.exclude_topics; + node["exclude_services"] = record_options.exclude_services; node["node_prefix"] = record_options.node_prefix; node["compression_mode"] = record_options.compression_mode; node["compression_format"] = record_options.compression_format; @@ -66,15 +69,18 @@ Node convert::encode( bool convert::decode( const Node & node, rosbag2_transport::RecordOptions & record_options) { - optional_assign(node, "all", record_options.all); + optional_assign(node, "all_topics", record_options.all_topics); + optional_assign(node, "all_services", record_options.all_services); optional_assign(node, "is_discovery_disabled", record_options.is_discovery_disabled); optional_assign>(node, "topics", record_options.topics); + optional_assign>(node, "services", record_options.services); optional_assign( node, "rmw_serialization_format", record_options.rmw_serialization_format); optional_assign( node, "topic_polling_interval", record_options.topic_polling_interval); optional_assign(node, "regex", record_options.regex); - optional_assign(node, "exclude", record_options.exclude); + optional_assign(node, "exclude_topics", record_options.exclude_topics); + optional_assign(node, "exclude_services", record_options.exclude_services); optional_assign(node, "node_prefix", record_options.node_prefix); optional_assign(node, "compression_mode", record_options.compression_mode); optional_assign(node, "compression_format", record_options.compression_format); diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 69c280e136..dac7496c1f 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -31,6 +31,7 @@ #include "rosbag2_cpp/bag_events.hpp" #include "rosbag2_cpp/writer.hpp" +#include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_interfaces/srv/snapshot.hpp" @@ -475,6 +476,13 @@ void RecorderImpl::subscribe_topic(const rosbag2_storage::TopicMetadata & topic) writer_->create_topic(topic); Rosbag2QoS subscription_qos{subscription_qos_for_topic(topic.name)}; + + // For service event topic, avoid receiving the last response message. + // TODO(Barry-Xu-2018): is there a better way ? + if (rosbag2_cpp::is_service_event_topic(topic.name, topic.type)) { + subscription_qos.durability(RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + auto subscription = create_subscription(topic.name, topic.type, subscription_qos); if (subscription) { subscriptions_.insert({topic.name, subscription}); diff --git a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp index a09f6b70c3..aee426d447 100644 --- a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp +++ b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp @@ -23,6 +23,7 @@ #include "rclcpp/node_interfaces/node_graph_interface.hpp" #include "rcpputils/split.hpp" #include "rosbag2_cpp/typesupport_helpers.hpp" +#include "rosbag2_cpp/service_utils.hpp" #include "logging.hpp" #include "rosbag2_transport/topic_filter.hpp" @@ -70,6 +71,18 @@ bool topic_in_list(const std::string & topic_name, const std::vector & service_event_topics) +{ + size_t pos = topic_name.rfind(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); + if (pos == std::string::npos) { + return false; + } + auto it = std::find(service_event_topics.begin(), service_event_topics.end(), topic_name); + return it != service_event_topics.end(); +} + bool topic_is_unpublished( const std::string & topic_name, rclcpp::node_interfaces::NodeGraphInterface & node_graph) @@ -117,6 +130,13 @@ std::unordered_map TopicFilter::filter_topics( bool TopicFilter::take_topic( const std::string & topic_name, const std::vector & topic_types) { + if (!has_single_type(topic_name, topic_types)) { + return false; + } + + const std::string & topic_type = topic_types[0]; + bool is_service_event_topic = rosbag2_cpp::is_service_event_topic(topic_name, topic_type); + if (!record_options_.include_unpublished_topics && node_graph_ && topic_is_unpublished(topic_name, *node_graph_)) { @@ -129,36 +149,78 @@ bool TopicFilter::take_topic( return false; } - if (!record_options_.topics.empty() && !topic_in_list(topic_name, record_options_.topics)) { - return false; - } + if (!is_service_event_topic) { + if (!record_options_.all_topics && + record_options_.topics.empty() && + record_options_.regex.empty() && + !record_options_.include_hidden_topics) + { + return false; + } - std::regex exclude_regex(record_options_.exclude); - if (!record_options_.exclude.empty() && std::regex_search(topic_name, exclude_regex)) { - return false; - } + if (!record_options_.topics.empty() && !topic_in_list(topic_name, record_options_.topics)) { + return false; + } - std::regex include_regex(record_options_.regex); - if ( - !record_options_.all && // All takes precedence over regex - !record_options_.regex.empty() && // empty regex matches nothing, but should be ignored - !std::regex_search(topic_name, include_regex)) - { - return false; - } + std::regex exclude_topics_regex(record_options_.exclude_topics); + if (!record_options_.exclude_topics.empty() && + std::regex_search(topic_name, exclude_topics_regex)) + { + return false; + } - if (!has_single_type(topic_name, topic_types)) { - return false; + std::regex include_regex(record_options_.regex); + if (!record_options_.all_topics && // All takes precedence over regex + !record_options_.regex.empty() && // empty regex matches nothing, but should be ignored + !std::regex_search(topic_name, include_regex)) + { + return false; + } + } else { + if (!record_options_.all_services && + record_options_.services.empty() && + record_options_.regex.empty()) + { + return false; + } + + if (!record_options_.services.empty() && + !service_in_list(topic_name, record_options_.services)) + { + return false; + } + + // Convert service event topic name to service name + auto service_name = + rosbag2_cpp::service_event_topic_name_to_service_name(topic_name); + + std::regex exclude_services_regex(record_options_.exclude_services); + if (!record_options_.exclude_services.empty() && + std::regex_search(service_name, exclude_services_regex)) + { + return false; + } + + std::regex include_regex(record_options_.regex); + if ( + !record_options_.all_services && // All takes precedence over regex + !record_options_.regex.empty() && // empty regex matches nothing, but should be ignored + !std::regex_search(service_name, include_regex)) + { + return false; + } } - if (!record_options_.include_hidden_topics && topic_is_hidden(topic_name)) { + if (!record_options_.include_hidden_topics && + topic_is_hidden(topic_name) && + !is_service_event_topic) + { RCUTILS_LOG_WARN_ONCE_NAMED( ROSBAG2_TRANSPORT_PACKAGE_NAME, "Hidden topics are not recorded. Enable them with --include-hidden-topics"); return false; } - const std::string & topic_type = topic_types[0]; if (!allow_unknown_types_ && !type_is_known(topic_name, topic_type)) { return false; } diff --git a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp index 8e91d01763..d012075808 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_keyboard_controls.cpp @@ -186,7 +186,8 @@ TEST_F(RecordIntegrationTestFixture, test_keyboard_controls) auto writer = std::make_unique(std::move(mock_sequential_writer)); auto keyboard_handler = std::make_shared(); - rosbag2_transport::RecordOptions record_options = {true, false, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, false, {}, {}, "rmw_format", 100ms}; record_options.start_paused = true; auto recorder = std::make_shared( diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index baff0b1583..fc1a7ce220 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/service_client_manager.hpp" #include "rosbag2_test_common/wait_for.hpp" #include "rosbag2_transport/recorder.hpp" @@ -46,7 +47,7 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are pub_manager.setup_publisher(string_topic, string_message, 2); rosbag2_transport::RecordOptions record_options = - {false, false, {string_topic, array_topic}, "rmw_format", 50ms}; + {false, false, false, {string_topic, array_topic}, {}, "rmw_format", 50ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -97,7 +98,7 @@ TEST_F(RecordIntegrationTestFixture, can_record_again_after_stop) pub_manager.setup_publisher(string_topic, string_message, 2); rosbag2_transport::RecordOptions record_options = - {false, false, {string_topic}, "rmw_format", 50ms}; + {false, false, false, {string_topic}, {}, "rmw_format", 50ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -149,7 +150,7 @@ TEST_F(RecordIntegrationTestFixture, qos_is_stored_in_metadata) pub_manager.setup_publisher(topic, string_message, 2); rosbag2_transport::RecordOptions record_options = - {false, false, {topic}, "rmw_format", 100ms}; + {false, false, false, {topic}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -211,7 +212,7 @@ TEST_F(RecordIntegrationTestFixture, records_sensor_data) pub_manager.setup_publisher(topic, string_message, 2, rclcpp::SensorDataQoS()); rosbag2_transport::RecordOptions record_options = - {false, false, {topic}, "rmw_format", 100ms}; + {false, false, false, {topic}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -254,7 +255,7 @@ TEST_F(RecordIntegrationTestFixture, receives_latched_messages) pub_manager.run_publishers(); rosbag2_transport::RecordOptions record_options = - {false, false, {topic}, "rmw_format", 100ms}; + {false, false, false, {topic}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -298,7 +299,8 @@ TEST_F(RecordIntegrationTestFixture, mixed_qos_subscribes) { auto publisher_transient_local = publisher_node->create_publisher( topic, profile_transient_local); - rosbag2_transport::RecordOptions record_options = {false, false, {topic}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {false, false, false, {topic}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -344,7 +346,8 @@ TEST_F(RecordIntegrationTestFixture, duration_and_noncompatibility_policies_mixe .liveliness(liveliness).liveliness_lease_duration(liveliness_lease_duration); auto publisher_liveliness = create_pub(profile_liveliness); - rosbag2_transport::RecordOptions record_options = {false, false, {topic}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {false, false, false, {topic}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -382,7 +385,7 @@ TEST_F(RecordIntegrationTestFixture, write_split_callback_is_called) mock_writer.set_max_messages_per_file(5); rosbag2_transport::RecordOptions record_options = - {false, false, {string_topic}, "rmw_format", 100ms}; + {false, false, false, {string_topic}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp index 5c72b4c6e4..a5b3933dba 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/service_client_manager.hpp" #include "rosbag2_transport/recorder.hpp" @@ -47,7 +49,8 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are pub_manager.setup_publisher(array_topic, array_message, 2); pub_manager.setup_publisher(string_topic, string_message, 2); - rosbag2_transport::RecordOptions record_options = {true, false, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, false, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); @@ -85,3 +88,83 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are EXPECT_THAT(array_messages[0]->bool_values, ElementsAre(true, false, true)); EXPECT_THAT(array_messages[0]->float32_values, ElementsAre(40.0f, 2.0f, 0.0f)); } + +TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_services_are_recorded) +{ + auto service_client_manager_1 = + std::make_shared>( + "test_service_1"); + + auto service_client_manager_2 = + std::make_shared>( + "test_service_2"); + + rosbag2_transport::RecordOptions record_options = + {false, true, false, {}, {}, "rmw_format", 100ms}; + auto recorder = std::make_shared( + std::move(writer_), storage_options_, record_options); + recorder->record(); + + start_async_spin(recorder); + + ASSERT_TRUE(service_client_manager_1->check_service_ready()); + + ASSERT_TRUE(service_client_manager_2->check_service_ready()); + + // By default, only client introspection is enable. + // For one request, service event topic get 2 messages. + ASSERT_TRUE(service_client_manager_1->send_request()); + ASSERT_TRUE(service_client_manager_2->send_request()); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + auto & writer = recorder->get_writer_handle(); + MockSequentialWriter & mock_writer = + static_cast(writer.get_implementation_handle()); + + size_t expected_messages = 4; + auto recorded_messages = mock_writer.get_messages(); + EXPECT_EQ(recorded_messages.size(), expected_messages); +} + +TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_are_recorded) +{ + auto service_client_manager_1 = + std::make_shared>( + "test_service"); + + auto string_message = get_messages_strings()[0]; + string_message->string_value = "Hello World"; + std::string string_topic = "/string_topic"; + rosbag2_test_common::PublicationManager pub_manager; + pub_manager.setup_publisher(string_topic, string_message, 1); + + rosbag2_transport::RecordOptions record_options = + {true, true, false, {}, {}, "rmw_format", 100ms}; + record_options.exclude_topics = "rosout"; + auto recorder = std::make_shared( + std::move(writer_), storage_options_, record_options); + recorder->record(); + + start_async_spin(recorder); + + ASSERT_TRUE(pub_manager.wait_for_matched(string_topic.c_str())); + + ASSERT_TRUE(service_client_manager_1->check_service_ready()); + + pub_manager.run_publishers(); + + // By default, only client introspection is enable. + // For one request, service event topic get 2 messages. + ASSERT_TRUE(service_client_manager_1->send_request()); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + auto & writer = recorder->get_writer_handle(); + MockSequentialWriter & mock_writer = + static_cast(writer.get_implementation_handle()); + + size_t expected_messages = 3; + auto recorded_messages = mock_writer.get_messages(); + EXPECT_EQ(recorded_messages.size(), expected_messages); +} diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp index 977aacfe6d..292cfbdd86 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all_ignore_leaf_topics.cpp @@ -51,7 +51,8 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_two_topics_ignore_l pub_manager.setup_publisher(array_topic, array_message, 2); pub_manager.setup_publisher(string_topic, string_message, 2); - rosbag2_transport::RecordOptions record_options = {true, false, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, false, {}, {}, "rmw_format", 100ms}; record_options.ignore_leaf_topics = true; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all_include_unpublished_topics.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_include_unpublished_topics.cpp index 809a1c6ad5..644840d976 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all_include_unpublished_topics.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all_include_unpublished_topics.cpp @@ -34,7 +34,8 @@ TEST_F(RecordIntegrationTestFixture, record_all_include_unpublished_false_ignore auto string_msgs_sub = node->create_subscription( string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {}); - rosbag2_transport::RecordOptions record_options = {true, false, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, false, {}, {}, "rmw_format", 100ms}; record_options.include_unpublished_topics = false; auto recorder = std::make_shared(writer_, storage_options_, record_options); recorder->record(); @@ -51,7 +52,8 @@ TEST_F(RecordIntegrationTestFixture, record_all_include_unpublished_true_include auto string_msgs_sub = node->create_subscription( string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {}); - rosbag2_transport::RecordOptions record_options = {true, false, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, false, {}, {}, "rmw_format", 100ms}; record_options.include_unpublished_topics = true; auto recorder = std::make_shared(writer_, storage_options_, record_options); recorder->record(); @@ -70,7 +72,8 @@ TEST_F( auto string_msgs_sub = node->create_subscription( string_topic, 10, [](test_msgs::msg::Strings::ConstSharedPtr) {}); - rosbag2_transport::RecordOptions record_options = {true, false, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, false, {}, {}, "rmw_format", 100ms}; record_options.include_unpublished_topics = false; auto recorder = std::make_shared(writer_, storage_options_, record_options); recorder->record(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp index e4aee9171b..845206cbeb 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all_no_discovery.cpp @@ -37,7 +37,8 @@ TEST_F(RecordIntegrationTestFixture, record_all_without_discovery_ignores_later_ auto string_message = get_messages_strings()[0]; string_message->string_value = "Hello World"; - rosbag2_transport::RecordOptions record_options = {true, true, {}, "rmw_format", 100ms}; + rosbag2_transport::RecordOptions record_options = + {true, false, true, {}, {}, "rmw_format", 100ms}; auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); recorder->record(); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp index f512307219..a2b9a9f192 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all_use_sim_time.cpp @@ -91,7 +91,7 @@ TEST_F(RecordIntegrationTestFixture, record_all_with_sim_time) rosbag2_transport::RecordOptions record_options = { - false, false, {string_topic, clock_topic}, "rmw_format", 100ms + false, false, false, {string_topic, clock_topic}, {}, "rmw_format", 100ms }; record_options.use_sim_time = true; auto recorder = std::make_shared( diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp index e305229152..157aeda063 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_options.cpp @@ -21,13 +21,16 @@ using namespace ::testing; // NOLINT TEST(record_options, test_yaml_serialization) { rosbag2_transport::RecordOptions original; - original.all = true; + original.all_topics = true; + original.all_services = true; original.is_discovery_disabled = true; original.topics = {"topic", "other_topic"}; + original.services = {"service", "other_service"}; original.rmw_serialization_format = "cdr"; original.topic_polling_interval = std::chrono::milliseconds{200}; original.regex = "[xyz]/topic"; - original.exclude = "*"; + original.exclude_topics = "*"; + original.exclude_services = "*"; original.node_prefix = "prefix"; original.compression_mode = "stream"; original.compression_format = "h264"; @@ -45,9 +48,11 @@ TEST(record_options, test_yaml_serialization) auto reconstructed = reconstructed_node.as(); #define CHECK(field) ASSERT_EQ(original.field, reconstructed.field) - CHECK(all); + CHECK(all_topics); + CHECK(all_services); CHECK(is_discovery_disabled); CHECK(topics); + CHECK(services); CHECK(rmw_serialization_format); #undef CMP } diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp index a20e48cdf4..52c21b1060 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/service_client_manager.hpp" #include "rosbag2_transport/recorder.hpp" #include "test_msgs/msg/arrays.hpp" #include "test_msgs/msg/basic_types.hpp" #include "test_msgs/message_fixtures.hpp" +#include "test_msgs/srv/basic_types.hpp" #include "record_integration_fixture.hpp" @@ -58,7 +60,8 @@ TEST_F(RecordIntegrationTestFixture, regex_topics_recording) ASSERT_FALSE(std::regex_match(b3, re)); ASSERT_FALSE(std::regex_match(b4, re)); - rosbag2_transport::RecordOptions record_options = {false, false, {}, "rmw_format", 10ms}; + rosbag2_transport::RecordOptions record_options = + {false, false, false, {}, {}, "rmw_format", 10ms}; record_options.regex = regex; // TODO(karsten1987) Refactor this into publication manager @@ -99,7 +102,7 @@ TEST_F(RecordIntegrationTestFixture, regex_topics_recording) EXPECT_TRUE(recorded_topics.find(v1) != recorded_topics.end()); } -TEST_F(RecordIntegrationTestFixture, regex_and_exclude_recording) +TEST_F(RecordIntegrationTestFixture, regex_and_exclude_topic_recording) { auto test_string_messages = get_messages_strings(); auto test_array_messages = get_messages_arrays(); @@ -129,9 +132,10 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_recording) ASSERT_TRUE(std::regex_match(e1, re)); ASSERT_TRUE(std::regex_match(e1, exclude)); - rosbag2_transport::RecordOptions record_options = {false, false, {}, "rmw_format", 10ms}; + rosbag2_transport::RecordOptions record_options = + {false, false, false, {}, {}, "rmw_format", 10ms}; record_options.regex = regex; - record_options.exclude = topics_regex_to_exclude; + record_options.exclude_topics = topics_regex_to_exclude; // TODO(karsten1987) Refactor this into publication manager @@ -174,3 +178,69 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_recording) EXPECT_TRUE(recorded_topics.find(v1) != recorded_topics.end()); EXPECT_TRUE(recorded_topics.find(v2) != recorded_topics.end()); } + +TEST_F(RecordIntegrationTestFixture, regex_and_exclude_service_recording) +{ + std::string regex = "/[a-z]+_nice(_.*)"; + std::string services_regex_to_exclude = "/[a-z]+_nice_[a-z]+/(.*)"; + + // matching service + std::string v1 = "/awesome_nice_service"; + std::string v2 = "/still_nice_service"; + + // excluded topics + std::string e1 = "/quite_nice_namespace/but_it_is_excluded"; + + // service that shouldn't match + std::string b1 = "/numberslike1arenot_nice"; + std::string b2 = "/namespace_before/not_nice"; + + rosbag2_transport::RecordOptions record_options = + {false, false, false, {}, {}, "rmw_format", 10ms}; + record_options.regex = regex; + record_options.exclude_services = services_regex_to_exclude; + + auto service_manager_v1 = + std::make_shared>(v1); + + auto service_manager_v2 = + std::make_shared>(v2); + + auto service_manager_e1 = + std::make_shared>(e1); + + auto service_manager_b1 = + std::make_shared>(b1); + + auto service_manager_b2 = + std::make_shared>(b2); + + auto recorder = std::make_shared( + std::move(writer_), storage_options_, record_options); + recorder->record(); + + start_async_spin(recorder); + + ASSERT_TRUE(service_manager_v1->check_service_ready()); + ASSERT_TRUE(service_manager_v2->check_service_ready()); + ASSERT_TRUE(service_manager_e1->check_service_ready()); + ASSERT_TRUE(service_manager_b1->check_service_ready()); + ASSERT_TRUE(service_manager_b2->check_service_ready()); + + auto & writer = recorder->get_writer_handle(); + MockSequentialWriter & mock_writer = + static_cast(writer.get_implementation_handle()); + + ASSERT_TRUE(service_manager_v1->send_request()); + ASSERT_TRUE(service_manager_v2->send_request()); + ASSERT_TRUE(service_manager_e1->send_request()); + ASSERT_TRUE(service_manager_b1->send_request()); + ASSERT_TRUE(service_manager_b2->send_request()); + auto recorded_messages = mock_writer.get_messages(); + EXPECT_THAT(recorded_messages, SizeIs(4)); + + auto recorded_topics = mock_writer.get_topics(); + EXPECT_THAT(recorded_topics, SizeIs(2)); + EXPECT_TRUE(recorded_topics.find(v1 + "/_service_event") != recorded_topics.end()); + EXPECT_TRUE(recorded_topics.find(v2 + "/_service_event") != recorded_topics.end()); +} diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp index 16d31835e7..32b78f27a6 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp @@ -68,7 +68,7 @@ class RecordSrvsTest : public RecordIntegrationTestFixture client_node_ = std::make_shared("test_record_client"); rosbag2_transport::RecordOptions record_options = - {false, false, {test_topic_}, "rmw_format", 100ms}; + {false, false, false, {test_topic_}, {}, "rmw_format", 100ms}; storage_options_.snapshot_mode = snapshot_mode_; storage_options_.max_cache_size = 200; recorder_ = std::make_shared( diff --git a/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp b/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp index 3271d12c47..b8ae64d6bd 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_rewrite.cpp @@ -93,7 +93,7 @@ TEST_P(TestRewrite, test_noop_rewrite) { output_storage.uri = (output_dir_ / "unchanged").string(); output_storage.storage_id = storage_id_; rosbag2_transport::RecordOptions output_record; - output_record.all = true; + output_record.all_topics = true; output_bags_.push_back({output_storage, output_record}); rosbag2_transport::bag_rewrite(input_bags_, output_bags_); @@ -114,7 +114,7 @@ TEST_P(TestRewrite, test_merge) { output_storage.uri = (output_dir_ / "merged").string(); output_storage.storage_id = storage_id_; rosbag2_transport::RecordOptions output_record; - output_record.all = true; + output_record.all_topics = true; output_bags_.push_back({output_storage, output_record}); rosbag2_transport::bag_rewrite(input_bags_, output_bags_); @@ -146,7 +146,7 @@ TEST_P(TestRewrite, test_message_definitions_stored_with_merge) { output_storage.uri = (output_dir_ / "merged").string(); output_storage.storage_id = storage_id_; rosbag2_transport::RecordOptions output_record; - output_record.all = true; + output_record.all_topics = true; output_bags_.push_back({output_storage, output_record}); rosbag2_transport::bag_rewrite(input_bags_, output_bags_); @@ -190,8 +190,8 @@ TEST_P(TestRewrite, test_filter_split) { storage_opts.uri = (output_dir_ / "split1").string(); storage_opts.storage_id = storage_id_; rosbag2_transport::RecordOptions rec_opts; - rec_opts.all = true; - rec_opts.exclude = "basic"; + rec_opts.all_topics = true; + rec_opts.exclude_topics = "basic"; output_bags_.push_back({storage_opts, rec_opts}); } { @@ -199,7 +199,7 @@ TEST_P(TestRewrite, test_filter_split) { storage_opts.uri = (output_dir_ / "split2").string(); storage_opts.storage_id = storage_id_; rosbag2_transport::RecordOptions rec_opts; - rec_opts.all = false; + rec_opts.all_topics = false; rec_opts.topics = {"b_basictypes"}; output_bags_.push_back({storage_opts, rec_opts}); } @@ -234,7 +234,7 @@ TEST_P(TestRewrite, test_compress) { output_storage.uri = out_bag.string(); output_storage.storage_id = storage_id_; rosbag2_transport::RecordOptions output_record; - output_record.all = true; + output_record.all_topics = true; output_record.compression_mode = "file"; output_record.compression_format = "zstd"; output_bags_.push_back({output_storage, output_record}); diff --git a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp index a4ce8c2fea..05a3b681a8 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp @@ -35,7 +35,10 @@ class RegexFixture : public Test {"/invalidated_topic", {"invalidated_topic_type"}}, {"/localization", {"localization_topic_type"}}, {"/invisible", {"invisible_topic_type"}}, - {"/status", {"status_topic_type"}} + {"/status", {"status_topic_type"}}, + {"/invalid_service/_service_event", {"service/srv/invalid_service_Event"}}, + {"/invalidated_service/_service_event", {"service/srv/invalidated_service_Event"}}, + {"/planning_service/_service_event", {"service/srv/planning_service_Event"}} }; }; @@ -58,6 +61,7 @@ TEST(TestTopicFilter, filter_hidden_topics) { } { rosbag2_transport::RecordOptions record_options; + record_options.all_topics = true; record_options.include_hidden_topics = false; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types); @@ -72,8 +76,9 @@ TEST(TestTopicFilter, filter_topics_with_more_than_one_type) { {"topic/c", {"type_c", "type_c2"}}, {"topic/d", {"type_d", "type_d", "type_d2"}}, }; - - rosbag2_transport::TopicFilter filter{rosbag2_transport::RecordOptions{}, nullptr, true}; + rosbag2_transport::RecordOptions record_options; + record_options.all_topics = true; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types); EXPECT_THAT(filtered_topics, SizeIs(2)); for (const auto & topic : @@ -89,8 +94,9 @@ TEST(TestTopicFilter, filter_topics_with_known_type_invalid) { {"topic/b", {"type_b"}}, {"topic/c", {"type_c"}} }; - - rosbag2_transport::TopicFilter filter{rosbag2_transport::RecordOptions{}, nullptr}; + rosbag2_transport::RecordOptions record_options; + record_options.all_topics = true; + rosbag2_transport::TopicFilter filter{record_options, nullptr}; auto filtered_topics = filter.filter_topics(topics_and_types); ASSERT_EQ(0u, filtered_topics.size()); } @@ -101,7 +107,9 @@ TEST(TestTopicFilter, filter_topics_with_known_type_valid) { {"topic/b", {"test_msgs/BasicTypes"}}, {"topic/c", {"test_msgs/BasicTypes"}} }; - rosbag2_transport::TopicFilter filter{rosbag2_transport::RecordOptions{}, nullptr}; + rosbag2_transport::RecordOptions record_options; + record_options.all_topics = true; + rosbag2_transport::TopicFilter filter{record_options, nullptr}; auto filtered_topics = filter.filter_topics(topics_and_types); ASSERT_EQ(3u, filtered_topics.size()); } @@ -110,7 +118,10 @@ TEST(TestTopicFilter, filter_topics) { std::map> topics_and_types { {"topic/a", {"type_a"}}, {"topic/b", {"type_b"}}, - {"topic/c", {"type_c"}} + {"topic/c", {"type_c"}}, + {"/service/a/_service_event", {"service/srv/type_a_Event"}}, + {"/service/b/_service_event", {"service/srv/type_b_Event"}}, + {"/service/c/_service_event", {"service/srv/type_c_Event"}}, }; { @@ -151,13 +162,38 @@ TEST(TestTopicFilter, filter_topics) { EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()); } } + + { + rosbag2_transport::RecordOptions record_options; + record_options.services = {"/service/a/_service_event"}; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types); + ASSERT_EQ(1u, filtered_topics.size()); + EXPECT_EQ("/service/a/_service_event", filtered_topics.begin()->first); + } + + { + rosbag2_transport::RecordOptions record_options; + record_options.services = { + "/service/a/_service_event", + "/service/b/_service_event", + "/service/d/_service_event"}; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types); + ASSERT_EQ(2u, filtered_topics.size()); + for (const auto & topic : + {"/service/a/_service_event", "/service/b/_service_event"}) + { + EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()); + } + } } -TEST_F(RegexFixture, regex_all_and_exclude) +TEST_F(RegexFixture, regex_all_topics_and_exclude_topics) { rosbag2_transport::RecordOptions record_options; - record_options.exclude = "/inv.*"; - record_options.all = true; + record_options.exclude_topics = "/inv.*"; + record_options.all_topics = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_); @@ -167,38 +203,100 @@ TEST_F(RegexFixture, regex_all_and_exclude) } } -TEST_F(RegexFixture, regex_filter_exclude) +TEST_F(RegexFixture, regex_all_services_and_exclude_services) { rosbag2_transport::RecordOptions record_options; - record_options.regex = "/invalid.*"; - record_options.exclude = ".invalidated.*"; - record_options.all = false; + record_options.exclude_services = "/inv.*"; + record_options.all_services = true; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_); EXPECT_THAT(filtered_topics, SizeIs(1)); + EXPECT_EQ("/planning_service/_service_event", filtered_topics.begin()->first); +} + +TEST_F(RegexFixture, regex_all_topics_all_services_and_exclude_topics_and_services) +{ + rosbag2_transport::RecordOptions record_options; + record_options.exclude_topics = "/inv.*"; + record_options.all_topics = true; + record_options.exclude_services = "/inv.*"; + record_options.all_services = true; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types_); + + EXPECT_THAT(filtered_topics, SizeIs(4)); + for (const auto & topic : + {"/planning", "/localization", "/status", "/planning_service/_service_event"}) + { + EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()); + } +} + +TEST_F(RegexFixture, regex_filter_exclude_topics) +{ + rosbag2_transport::RecordOptions record_options; + record_options.regex = "/invalid.*"; + record_options.exclude_topics = ".invalidated.*"; // Only affect topics + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types_); + + EXPECT_THAT(filtered_topics, SizeIs(3)); EXPECT_TRUE(filtered_topics.find("/invalid_topic") != filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find("/invalid_service/_service_event") != filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find("/invalidated_service/_service_event") != filtered_topics.end()); +} + +TEST_F(RegexFixture, regex_filter_exclude_services) +{ + rosbag2_transport::RecordOptions record_options; + record_options.regex = "/invalid.*"; + record_options.exclude_services = ".invalidated.*"; // Only affect services + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types_); + + EXPECT_THAT(filtered_topics, SizeIs(3)); + EXPECT_TRUE(filtered_topics.find("/invalid_topic") != filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find("/invalidated_topic") != filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find("/invalid_service/_service_event") != filtered_topics.end()); +} + +TEST_F(RegexFixture, regex_filter_exclude_topics_and_services) +{ + rosbag2_transport::RecordOptions record_options; + record_options.regex = "/invalid.*"; + record_options.exclude_topics = ".invalidated.*"; + record_options.exclude_services = ".invalidated.*"; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types_); + + EXPECT_THAT(filtered_topics, SizeIs(2)); + EXPECT_TRUE(filtered_topics.find("/invalid_topic") != filtered_topics.end()); + EXPECT_TRUE(filtered_topics.find("/invalid_service/_service_event") != filtered_topics.end()); } TEST_F(RegexFixture, regex_filter) { rosbag2_transport::RecordOptions record_options; record_options.regex = "^/inval"; - record_options.all = false; rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_); - EXPECT_THAT(filtered_topics, SizeIs(2)); - for (const auto & topic : {"/invalid_topic", "/invalidated_topic"}) { + EXPECT_THAT(filtered_topics, SizeIs(4)); + for (const auto & topic : + {"/invalid_topic", "/invalidated_topic", "/invalid_service/_service_event", + "/invalidated_service/_service_event"}) + { EXPECT_TRUE(filtered_topics.find(topic) != filtered_topics.end()); } } -TEST_F(RegexFixture, regex_all_and_filter) +TEST_F(RegexFixture, regex_all_topics_and_filter) { rosbag2_transport::RecordOptions record_options; record_options.regex = "/status"; - record_options.all = true; + record_options.all_topics = true; + record_options.exclude_services = ".*"; // Not include services rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; auto filtered_topics = filter.filter_topics(topics_and_types_); EXPECT_THAT(filtered_topics, SizeIs(6)); @@ -241,3 +339,25 @@ TEST_F(RegexFixture, do_not_print_warning_about_unknown_types_if_topic_is_not_se "Topic '/planning' has unknown type 'planning_topic_type'") != std::string::npos); } } + +TEST_F(RegexFixture, regex_all_services_and_filter) +{ + rosbag2_transport::RecordOptions record_options; + record_options.regex = "/status"; + record_options.all_services = true; + record_options.exclude_topics = ".*"; // Not include topics + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types_); + EXPECT_THAT(filtered_topics, SizeIs(3)); +} + +TEST_F(RegexFixture, regex_all_topics_all_services_and_filter) +{ + rosbag2_transport::RecordOptions record_options; + record_options.regex = "/status"; + record_options.all_topics = true; + record_options.all_services = true; + rosbag2_transport::TopicFilter filter{record_options, nullptr, true}; + auto filtered_topics = filter.filter_topics(topics_and_types_); + EXPECT_THAT(filtered_topics, SizeIs(9)); +} diff --git a/rosbag2_transport/test/srv/SimpleTest.srv b/rosbag2_transport/test/srv/SimpleTest.srv new file mode 100644 index 0000000000..afdafd5367 --- /dev/null +++ b/rosbag2_transport/test/srv/SimpleTest.srv @@ -0,0 +1,3 @@ +int64 input +--- +int64 output \ No newline at end of file From 2527eb324e99f874514981e9252a6f05fe0706e1 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Mon, 31 Jul 2023 16:42:14 +0800 Subject: [PATCH 2/8] Implement service record phase 2 Signed-off-by: Barry Xu --- ros2bag/ros2bag/verb/burst.py | 15 +- ros2bag/ros2bag/verb/play.py | 27 +- rosbag2_py/src/rosbag2_py/_storage.cpp | 17 +- rosbag2_py/src/rosbag2_py/_transport.cpp | 4 +- .../rosbag2_storage/storage_filter.hpp | 20 +- rosbag2_storage_mcap/src/mcap_storage.cpp | 61 ++- .../sqlite_storage.cpp | 43 +- .../test_sqlite_storage.cpp | 2 +- ..._client_manager.hpp => client_manager.hpp} | 11 +- .../rosbag2_test_common/service_manager.hpp | 89 +++++ .../rosbag2_transport/play_options.hpp | 18 +- .../include/rosbag2_transport/player.hpp | 6 + .../src/rosbag2_transport/player.cpp | 368 +++++++++++++++--- .../test/rosbag2_transport/mock_player.hpp | 12 + .../mock_sequential_reader.hpp | 18 +- .../rosbag2_play_test_fixture.hpp | 3 + .../test/rosbag2_transport/test_burst.cpp | 52 +++ .../test/rosbag2_transport/test_play.cpp | 360 +++++++++++++++++ .../test/rosbag2_transport/test_play_loop.cpp | 4 +- .../test/rosbag2_transport/test_record.cpp | 2 +- .../rosbag2_transport/test_record_all.cpp | 26 +- .../rosbag2_transport/test_record_regex.cpp | 12 +- 22 files changed, 1055 insertions(+), 115 deletions(-) rename rosbag2_test_common/include/rosbag2_test_common/{service_client_manager.hpp => client_manager.hpp} (93%) create mode 100644 rosbag2_test_common/include/rosbag2_test_common/service_manager.hpp diff --git a/ros2bag/ros2bag/verb/burst.py b/ros2bag/ros2bag/verb/burst.py index c2c0876879..bbd9f8589c 100644 --- a/ros2bag/ros2bag/verb/burst.py +++ b/ros2bag/ros2bag/verb/burst.py @@ -41,8 +41,12 @@ def add_arguments(self, parser, cli_name): # noqa: D102 'delay of message playback.') parser.add_argument( '--topics', type=str, default=[], nargs='+', - help='topics to replay, separated by space. If none specified, all topics will be ' - 'replayed.') + help='topics to replay, separated by space. At least one topic needs to be ' + "specified. If this parameter isn\'t specified, all topics will be replayed.") + parser.add_argument( + '--services', type=str, default=[], nargs='+', + help='services to replay, separated by space. At least one service needs to be ' + "specified. If this parameter isn\'t specified, all services will be replayed.") parser.add_argument( '--qos-profile-overrides-path', type=FileType('r'), help='Path to a yaml file defining overrides of the QoS profile for specific topics.') @@ -90,6 +94,13 @@ def main(self, *, args): # noqa: D102 play_options.node_prefix = NODE_NAME_PREFIX play_options.rate = 1.0 play_options.topics_to_filter = args.topics + # Convert service name to service event topic name + services = [] + if args.services and len(args.services) != 0: + for s in args.services: + name = '/' + s if s[0] != '/' else s + services.append(name + '/_service_event') + play_options.services_to_filter = services play_options.topic_qos_profile_overrides = qos_profile_overrides play_options.loop = False play_options.topic_remapping_options = topic_remapping diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index b2c84f2e80..d186137038 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -51,14 +51,22 @@ def add_arguments(self, parser, cli_name): # noqa: D102 parser.add_argument( '--topics', type=str, default=[], nargs='+', help='Space-delimited list of topics to play.') + parser.add_argument( + '--services', type=str, default=[], nargs='+', + help='services to replay, separated by space. At least one service needs to be ' + 'specified.') parser.add_argument( '-e', '--regex', default='', help='filter topics by regular expression to replay, separated by space. If none ' 'specified, all topics will be replayed.') parser.add_argument( - '-x', '--exclude', default='', + '--exclude-topics', default='', help='regular expressions to exclude topics from replay, separated by space. If none ' 'specified, all topics will be replayed.') + parser.add_argument( + '--exclude-services', default='', + help='regular expressions to exclude services from replay, separated by space. If ' + 'none specified, all services will be replayed.') parser.add_argument( '--qos-profile-overrides-path', type=FileType('r'), help='Path to a yaml file defining overrides of the QoS profile for specific topics.') @@ -182,8 +190,21 @@ def main(self, *, args): # noqa: D102 play_options.node_prefix = NODE_NAME_PREFIX play_options.rate = args.rate play_options.topics_to_filter = args.topics - play_options.topics_regex_to_filter = args.regex - play_options.topics_regex_to_exclude = args.exclude + + # Convert service name to service event topic name + services = [] + if args.services and len(args.services) != 0: + for s in args.services: + name = '/' + s if s[0] != '/' else s + services.append(name + '/_service_event') + play_options.services_to_filter = services + + play_options.regex_to_filter = args.regex + play_options.topics_regex_to_exclude = args.exclude_topics + if args.exclude_services: + play_options.services_regex_to_exclude = args.exclude_services + '/_service_event' + else: + play_options.services_regex_to_exclude = args.exclude_services play_options.topic_qos_profile_overrides = qos_profile_overrides play_options.loop = args.loop play_options.topic_remapping_options = topic_remapping diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index 4ecdcb2c4f..109eb3cdda 100644 --- a/rosbag2_py/src/rosbag2_py/_storage.cpp +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -120,15 +120,22 @@ PYBIND11_MODULE(_storage, m) { pybind11::class_(m, "StorageFilter") .def( - pybind11::init, std::string, std::string>(), + pybind11::init< + std::vector, std::vector, std::string, std::string, std::string>(), pybind11::arg("topics") = std::vector(), - pybind11::arg("topics_regex") = "", - pybind11::arg("topics_regex_to_exclude") = "") + pybind11::arg("services") = std::vector(), + pybind11::arg("regex") = "", + pybind11::arg("topics_regex_to_exclude") = "", + pybind11::arg("services_regex_to_exclude") = "") .def_readwrite("topics", &rosbag2_storage::StorageFilter::topics) - .def_readwrite("topics_regex", &rosbag2_storage::StorageFilter::topics_regex) + .def_readwrite("services", &rosbag2_storage::StorageFilter::services) + .def_readwrite("regex", &rosbag2_storage::StorageFilter::regex) .def_readwrite( "topics_regex_to_exclude", - &rosbag2_storage::StorageFilter::topics_regex_to_exclude); + &rosbag2_storage::StorageFilter::topics_regex_to_exclude) + .def_readwrite( + "services_regex_to_exclude", + &rosbag2_storage::StorageFilter::services_regex_to_exclude); pybind11::class_(m, "MessageDefinition") .def( diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index d9ee4c3833..c17025f19d 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -318,8 +318,10 @@ PYBIND11_MODULE(_transport, m) { .def_readwrite("node_prefix", &PlayOptions::node_prefix) .def_readwrite("rate", &PlayOptions::rate) .def_readwrite("topics_to_filter", &PlayOptions::topics_to_filter) - .def_readwrite("topics_regex_to_filter", &PlayOptions::topics_regex_to_filter) + .def_readwrite("services_to_filter", &PlayOptions::services_to_filter) + .def_readwrite("regex_to_filter", &PlayOptions::regex_to_filter) .def_readwrite("topics_regex_to_exclude", &PlayOptions::topics_regex_to_exclude) + .def_readwrite("services_regex_to_exclude", &PlayOptions::services_regex_to_exclude) .def_property( "topic_qos_profile_overrides", &PlayOptions::getTopicQoSProfileOverrides, diff --git a/rosbag2_storage/include/rosbag2_storage/storage_filter.hpp b/rosbag2_storage/include/rosbag2_storage/storage_filter.hpp index 4c401505c2..6f5aedb3e1 100644 --- a/rosbag2_storage/include/rosbag2_storage/storage_filter.hpp +++ b/rosbag2_storage/include/rosbag2_storage/storage_filter.hpp @@ -25,18 +25,28 @@ struct StorageFilter { // Topic names to whitelist when reading a bag. Only messages matching these // specified topics will be returned. If list is empty, the filter is ignored - // and all messages are returned. + // and all messages of topics are returned. std::vector topics; - // Regular expression of topic names to whitelist when playing a bag. - // Only messages matching these specified topics will be played. + // Service names to whitelist when reading a bag. Only messages matching these + // specified service will be returned. If list is empty, the filter is ignored + // and all messages of services are returned. + std::vector services; + + // Regular expression of topic names and service name to whitelist when playing a bag. + // Only messages matching these specified topics or services will be played. // If list is empty, the filter is ignored and all messages are played. - std::string topics_regex = ""; + std::string regex = ""; // Regular expression of topic names to exclude when playing a bag. // Only messages not matching these specified topics will be played. - // If list is empty, the filter is ignored and all messages are played. + // If list is empty, the filter is ignored and all messages of topics are played. std::string topics_regex_to_exclude = ""; + + // Regular expression of topic names to exclude when playing a bag. + // Only messages not matching these specified services will be played. + // If list is empty, the filter is ignored and all messages of services are played. + std::string services_regex_to_exclude = ""; }; } // namespace rosbag2_storage diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index 716967a12b..b3fa432a1a 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -521,26 +521,63 @@ void MCAPStorage::reset_iterator() options.endTime = mcap::MaxTime; } options.readOrder = read_order_; - if (!storage_filter_.topics.empty()) { - options.topicFilter = [this](std::string_view topic) { + + auto filter_process = [this](std::string_view topic) { + if (!storage_filter_.topics.empty()) { for (const auto & match_topic : storage_filter_.topics) { if (match_topic == topic) { return true; } } - return false; - }; - } + } + + if (!storage_filter_.services.empty()) { + for (const auto & match_service : storage_filter_.services) { + if (match_service == topic) { + return true; + } + } + } + + bool topics_regex_to_exclude_match = false; + bool services_regex_to_exclude_match = false; + std::string topic_string(topic); + + if (!storage_filter_.topics_regex_to_exclude.empty()) { + std::smatch m; + std::regex re(storage_filter_.topics_regex_to_exclude); + topics_regex_to_exclude_match = std::regex_match(topic_string, m, re); + } + + if (!storage_filter_.services_regex_to_exclude.empty()) { + std::smatch m; + std::regex re(storage_filter_.services_regex_to_exclude); + services_regex_to_exclude_match = std::regex_match(topic_string, m, re); + } + #ifdef ROSBAG2_STORAGE_MCAP_HAS_STORAGE_FILTER_TOPIC_REGEX - if (!storage_filter_.topics_regex.empty()) { - options.topicFilter = [this](std::string_view topic) { + if (!storage_filter_.regex.empty()) { std::smatch m; - std::string topic_string(topic); - std::regex re(storage_filter_.topics_regex); - return std::regex_match(topic_string, m, re); - }; - } + std::regex re(storage_filter_.regex); + + if (std::regex_match(topic_string, m, re) && !topics_regex_to_exclude_match && + !services_regex_to_exclude_match) { + return true; + } else { + return false; + } + } #endif + + if ((storage_filter_.topics.empty() && !topics_regex_to_exclude_match) && + (storage_filter_.services.empty() && !services_regex_to_exclude_match)) { + return true; + } + + return false; + }; + options.topicFilter = filter_process; + linear_view_ = std::make_unique(mcap_reader_->readMessages(OnProblem, options)); linear_iterator_ = std::make_unique(linear_view_->begin()); diff --git a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp index d83e8d5883..8c4cb75f20 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -521,6 +521,7 @@ void SqliteStorage::prepare_for_reading() "FROM messages JOIN topics ON messages.topic_id = topics.id WHERE "; std::vector where_conditions; + std::string topic_and_service_list; // add topic filter if (!storage_filter_.topics.empty()) { // Construct string for selected topics @@ -531,13 +532,39 @@ void SqliteStorage::prepare_for_reading() topic_list += ","; } } - where_conditions.push_back("(topics.name IN (" + topic_list + "))"); + topic_and_service_list = "(topics.name IN (" + topic_list + "))"; } - // add topic filter based on regular expression - if (!storage_filter_.topics_regex.empty()) { + + // add service filter + if (!storage_filter_.services.empty()) { // Construct string for selected topics - where_conditions.push_back("(topics.name REGEXP '" + storage_filter_.topics_regex + "')"); + std::string service_list{""}; + for (auto & service : storage_filter_.services) { + service_list += "'" + service + "'"; + if (&service != &storage_filter_.topics.back()) { + service_list += ","; + } + } + + topic_and_service_list = topic_and_service_list + + std::string(topic_and_service_list.empty() ? "" : " OR ") + + "(topics.name IN (" + service_list + "))"; + } + + std::string list_and_regex = topic_and_service_list; + // add topic filter based on regular expression + if (!storage_filter_.regex.empty()) { + std::string regex = "(topics.name REGEXP '" + storage_filter_.regex + "')"; + list_and_regex = list_and_regex + + std::string(!list_and_regex.empty() ? " OR " : "") + + regex; } + + if (!list_and_regex.empty()) { + where_conditions.push_back(list_and_regex); + } + + std::string exclude_topics_services; // exclude topics based on regular expressions if (!storage_filter_.topics_regex_to_exclude.empty()) { // Construct string for selected topics @@ -546,6 +573,14 @@ void SqliteStorage::prepare_for_reading() "(SELECT topics.name FROM topics WHERE topics.name REGEXP '" + storage_filter_.topics_regex_to_exclude + "'))"); } + // exclude service based on regular expressions + if (!storage_filter_.services_regex_to_exclude.empty()) { + // Construct string for selected topics + where_conditions.push_back( + "(topics.name NOT IN " + "(SELECT topics.name FROM topics WHERE topics.name REGEXP '" + + storage_filter_.services_regex_to_exclude + "'))"); + } const std::string direction_op = read_order_.reverse ? "<" : ">"; const std::string order_direction = read_order_.reverse ? "DESC" : "ASC"; diff --git a/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_storage.cpp b/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_storage.cpp index ae8582d02f..3a2d985da5 100644 --- a/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_storage.cpp @@ -566,7 +566,7 @@ TEST_F(StorageTestFixture, read_next_returns_filtered_messages_regex) { readable_storage->open({db_filename, kPluginID}); rosbag2_storage::StorageFilter storage_filter; - storage_filter.topics_regex = "topic.*"; + storage_filter.regex = "topic.*"; readable_storage->set_filter(storage_filter); EXPECT_TRUE(readable_storage->has_next()); diff --git a/rosbag2_test_common/include/rosbag2_test_common/service_client_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp similarity index 93% rename from rosbag2_test_common/include/rosbag2_test_common/service_client_manager.hpp rename to rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp index 2862b1fb98..a7764b4e49 100644 --- a/rosbag2_test_common/include/rosbag2_test_common/service_client_manager.hpp +++ b/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef ROSBAG2_TEST_COMMON__SERVICE_CLIENT_MANAGER_HPP_ -#define ROSBAG2_TEST_COMMON__SERVICE_CLIENT_MANAGER_HPP_ +#ifndef ROSBAG2_TEST_COMMON__CLIENT_MANAGER_HPP_ +#define ROSBAG2_TEST_COMMON__CLIENT_MANAGER_HPP_ #include #include @@ -23,13 +23,14 @@ #include "rclcpp/rclcpp.hpp" // rclcpp must be included before the Windows specific includes. + namespace rosbag2_test_common { template -class ServiceClientManager : public rclcpp::Node +class ClientManager : public rclcpp::Node { public: - explicit ServiceClientManager( + explicit ClientManager( std::string service_name, size_t client_number = 1, bool service_event_contents = false, @@ -122,4 +123,4 @@ class ServiceClientManager : public rclcpp::Node }; } // namespace rosbag2_test_common -#endif // ROSBAG2_TEST_COMMON__SERVICE_CLIENT_MANAGER_HPP_ +#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_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index 4f880d0614..8d0ed448a1 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -37,19 +37,29 @@ struct PlayOptions // Topic names to whitelist when playing a bag. // Only messages matching these specified topics will be played. - // If list is empty, the filter is ignored and all messages are played. + // If list is empty, the filter is ignored and all messages of topics are played. std::vector topics_to_filter = {}; - // Regular expression of topic names to whitelist when playing a bag. - // Only messages matching these specified topics will be played. + // Service names to whitelist when playing a bag. + // Only messages matching these specified service will be played. + // If list is empty, the filter is ignored and all messages of services are played. + std::vector services_to_filter = {}; + + // Regular expression of topic names and service name to whitelist when playing a bag. + // Only messages matching these specified topics and services will be played. // If list is empty, the filter is ignored and all messages are played. - std::string topics_regex_to_filter = ""; + std::string regex_to_filter = ""; // Regular expression of topic names to exclude when playing a bag. // Only messages not matching these specified topics will be played. // If list is empty, the filter is ignored and all messages are played. std::string topics_regex_to_exclude = ""; + // Regular expression of service names to exclude when playing a bag. + // Only messages not matching these specified topics will be played. + // If list is empty, the filter is ignored and all messages are played. + std::string services_regex_to_exclude = ""; + std::unordered_map topic_qos_profile_overrides = {}; bool loop = false; std::vector topic_remapping_options = {}; diff --git a/rosbag2_transport/include/rosbag2_transport/player.hpp b/rosbag2_transport/include/rosbag2_transport/player.hpp index 59633cb14a..fcfc41ebe6 100644 --- a/rosbag2_transport/include/rosbag2_transport/player.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player.hpp @@ -55,6 +55,7 @@ #include "rosgraph_msgs/msg/clock.hpp" + namespace rosbag2_cpp { class Reader; @@ -208,6 +209,11 @@ class Player : public rclcpp::Node ROSBAG2_TRANSPORT_PUBLIC std::unordered_map> get_publishers(); + /// \brief Getter for clients corresponding to each service name + /// \return Hashtable representing service name to client + ROSBAG2_TRANSPORT_PUBLIC + std::unordered_map> get_clients(); + /// \brief Getter for inner clock_publisher /// \return Shared pointer to the inner clock_publisher ROSBAG2_TRANSPORT_PUBLIC diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 98c767e24e..cfd660eb11 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -21,22 +21,28 @@ #include #include #include +#include #include #include "rcl/graph.h" +#include "rclcpp/logger.hpp" #include "rclcpp/rclcpp.hpp" #include "rcutils/time.h" #include "rosbag2_cpp/clocks/time_controller_clock.hpp" #include "rosbag2_cpp/reader.hpp" +#include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_cpp/typesupport_helpers.hpp" #include "rosbag2_storage/storage_filter.hpp" #include "rosbag2_transport/qos.hpp" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" + namespace { /** @@ -183,6 +189,10 @@ class PlayerImpl /// \return Hashtable representing topic to publisher map excluding inner clock_publisher std::unordered_map> get_publishers(); + /// \brief Getter for clients corresponding to services + /// \return Hashtable representing service name to client map + std::unordered_map> get_clients(); + /// \brief Getter for inner clock_publisher /// \return Shared pointer to the inner clock_publisher rclcpp::Publisher::SharedPtr get_clock_publisher(); @@ -242,11 +252,57 @@ class PlayerImpl std::shared_ptr publisher_; std::function publish_func_; }; + + class PlayerClient final + { +public: + explicit PlayerClient( + std::shared_ptr cli, + std::string service_name, + std::string service_event_type, + const rclcpp::Logger & logger); + + // Can call this function if check_include_request_message() return true + void async_send_request(const rclcpp::SerializedMessage & message); + + std::shared_ptr generic_client() + { + return client_; + } + + // Check if message can be unpacked to get request message + bool is_include_request_message(const rclcpp::SerializedMessage & message); + +private: + std::shared_ptr client_; + std::string service_name_; + const rclcpp::Logger & logger_; + enum class introspection_type + { + UNKNOW, + METADATA, + CONTENTS + }; + introspection_type client_side_type_ = introspection_type::UNKNOW; + introspection_type service_side_type_ = introspection_type::UNKNOW; + + std::shared_ptr ts_lib_; + const rosidl_message_type_support_t * ts_; + const rosidl_typesupport_introspection_cpp::MessageMembers * message_members_; + + rcutils_allocator_t allocator_ = rcutils_get_default_allocator(); + + uint8_t get_msg_event_type(const rclcpp::SerializedMessage & message); + }; bool is_ready_to_play_from_queue_{false}; std::mutex ready_to_play_from_queue_mutex_; std::condition_variable ready_to_play_from_queue_cv_; rclcpp::Publisher::SharedPtr clock_publisher_; - std::unordered_map> publishers_; + using SharedPlayerPublisher = std::shared_ptr; + using SharedPlayerClient = std::shared_ptr; + std::unordered_map< + std::string, + std::variant> senders_; private: rosbag2_storage::SerializedBagMessageSharedPtr peek_next_message_from_queue(); @@ -451,13 +507,17 @@ bool PlayerImpl::play() if (timeout == std::chrono::milliseconds(0)) { timeout = std::chrono::milliseconds(-1); } - for (auto pub : publishers_) { + for (auto pub : senders_) { try { - if (!pub.second->generic_publisher()->wait_for_all_acked(timeout)) { - RCLCPP_ERROR( - owner_->get_logger(), - "Timed out while waiting for all published messages to be acknowledged for topic %s", - pub.first.c_str()); + if (pub.second.index() == 0) { // publisher + if (!std::get(pub.second) + ->generic_publisher()->wait_for_all_acked(timeout)) + { + RCLCPP_ERROR( + owner_->get_logger(), + "Timed out while waiting for all published messages to be acknowledged for topic %s", + pub.first.c_str()); + } } } catch (std::exception & e) { RCLCPP_ERROR( @@ -700,12 +760,28 @@ std::unordered_map> PlayerImpl::get_publishers() { std::unordered_map> topic_to_publisher_map; - for (const auto & [topic, publisher] : publishers_) { - topic_to_publisher_map[topic] = publisher->generic_publisher(); + for (const auto & [topic, sender] : senders_) { + if (std::holds_alternative(sender)) { + topic_to_publisher_map[topic] = + std::get(sender)->generic_publisher(); + } } return topic_to_publisher_map; } +std::unordered_map> PlayerImpl::get_clients() +{ + std::unordered_map> topic_to_client_map; + for (const auto & [service_name, sender] : senders_) { + if (std::holds_alternative(sender)) { + topic_to_client_map[service_name] = + std::get(sender)->generic_client(); + } + } + return topic_to_client_map; +} + rclcpp::Publisher::SharedPtr PlayerImpl::get_clock_publisher() { return clock_publisher_; @@ -833,8 +909,10 @@ void PlayerImpl::prepare_publishers() { rosbag2_storage::StorageFilter storage_filter; storage_filter.topics = play_options_.topics_to_filter; - storage_filter.topics_regex = play_options_.topics_regex_to_filter; + storage_filter.services = play_options_.services_to_filter; + storage_filter.regex = play_options_.regex_to_filter; storage_filter.topics_regex_to_exclude = play_options_.topics_regex_to_exclude; + storage_filter.services_regex_to_exclude = play_options_.services_regex_to_exclude; reader_->set_filter(storage_filter); // Create /clock publisher @@ -875,39 +953,64 @@ void PlayerImpl::prepare_publishers() auto topics = reader_->get_all_topics_and_types(); std::string topic_without_support_acked; for (const auto & topic : topics) { - if (publishers_.find(topic.name) != publishers_.end()) { + if (senders_.find(topic.name) != senders_.end()) { continue; } - // filter topics to add publishers if necessary + auto & filter_topics = storage_filter.topics; - if (!filter_topics.empty()) { - auto iter = std::find(filter_topics.begin(), filter_topics.end(), topic.name); - if (iter == filter_topics.end()) { - continue; + auto & filter_services = storage_filter.services; + + if (rosbag2_cpp::is_service_event_topic(topic.name, topic.type)) { + // filter services to add clients if necessary + if (!filter_services.empty()) { + auto iter = std::find(filter_services.begin(), filter_services.end(), topic.name); + if (iter == filter_services.end()) { + continue; + } } - } - - auto topic_qos = publisher_qos_for_topic( - topic, topic_qos_profile_overrides_, - owner_->get_logger()); - try { - std::shared_ptr pub = - owner_->create_generic_publisher(topic.name, topic.type, topic_qos); - std::shared_ptr player_pub = - std::make_shared( - std::move(pub), play_options_.disable_loan_message); - publishers_.insert(std::make_pair(topic.name, player_pub)); - if (play_options_.wait_acked_timeout >= 0 && - topic_qos.reliability() == rclcpp::ReliabilityPolicy::BestEffort) - { - topic_without_support_acked += topic.name + ", "; + auto service_name = rosbag2_cpp::service_event_topic_name_to_service_name(topic.name); + auto service_type = rosbag2_cpp::service_event_topic_type_to_service_type(topic.type); + try { + auto cli = owner_->create_generic_client(service_name, service_type); + auto player_cli = std::make_shared( + std::move(cli), service_name, topic.type, owner_->get_logger()); + senders_.insert(std::make_pair(topic.name, player_cli)); + } catch (const std::runtime_error & e) { + RCLCPP_WARN( + owner_->get_logger(), + "Ignoring a service '%s', reason: %s.", + service_name.c_str(), e.what()); + } + } else { + // filter topics to add publishers if necessary + if (!filter_topics.empty()) { + auto iter = std::find(filter_topics.begin(), filter_topics.end(), topic.name); + if (iter == filter_topics.end()) { + continue; + } + } + auto topic_qos = publisher_qos_for_topic( + topic, topic_qos_profile_overrides_, + owner_->get_logger()); + try { + std::shared_ptr pub = + owner_->create_generic_publisher(topic.name, topic.type, topic_qos); + std::shared_ptr player_pub = + std::make_shared( + std::move(pub), play_options_.disable_loan_message); + senders_.insert(std::make_pair(topic.name, player_pub)); + if (play_options_.wait_acked_timeout >= 0 && + topic_qos.reliability() == rclcpp::ReliabilityPolicy::BestEffort) + { + topic_without_support_acked += topic.name + ", "; + } + } catch (const std::runtime_error & e) { + // using a warning log seems better than adding a new option + // to ignore some unknown message type library + RCLCPP_WARN( + owner_->get_logger(), + "Ignoring a topic '%s', reason: %s.", topic.name.c_str(), e.what()); } - } catch (const std::runtime_error & e) { - // using a warning log seems better than adding a new option - // to ignore some unknown message type library - RCLCPP_WARN( - owner_->get_logger(), - "Ignoring a topic '%s', reason: %s.", topic.name.c_str(), e.what()); } } @@ -941,8 +1044,17 @@ void PlayerImpl::prepare_publishers() bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message) { bool message_published = false; - auto publisher_iter = publishers_.find(message->topic_name); - if (publisher_iter != publishers_.end()) { + + auto sender_iter = senders_.find(message->topic_name); + if (sender_iter != senders_.end()) { + // For sending requests, ignore service event messages that do not contain request information. + if (sender_iter->second.index() == 1 && + !std::get(sender_iter->second) + ->is_include_request_message(rclcpp::SerializedMessage(*message->serialized_data))) + { + return message_published; + } + { // Calling on play message pre-callbacks std::lock_guard lk(on_play_msg_callbacks_mutex_); for (auto & pre_callback_data : on_play_msg_pre_callbacks_) { @@ -952,13 +1064,34 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr } } - try { - publisher_iter->second->publish(rclcpp::SerializedMessage(*message->serialized_data)); - message_published = true; - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - owner_->get_logger(), "Failed to publish message on '" << message->topic_name << - "' topic. \nError: " << e.what()); + switch (sender_iter->second.index()) { + case 0: // publisher + { + try { + std::get(sender_iter->second) + ->publish(rclcpp::SerializedMessage(*message->serialized_data)); + message_published = true; + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + owner_->get_logger(), "Failed to publish message on '" << message->topic_name << + "' topic. \nError: " << e.what()); + } + } + break; + case 1: // Client + { + try { + std::get(sender_iter->second) + ->async_send_request(rclcpp::SerializedMessage(*message->serialized_data)); + message_published = true; + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + owner_->get_logger(), "Failed to send request on '" << + rosbag2_cpp::service_event_topic_name_to_service_name(message->topic_name) << + "' service. \nError: " << e.what()); + } + } + break; } // Calling on play message post-callbacks @@ -1304,6 +1437,11 @@ std::unordered_map> Playe return pimpl_->get_publishers(); } +std::unordered_map> Player::get_clients() +{ + return pimpl_->get_clients(); +} + rclcpp::Publisher::SharedPtr Player::get_clock_publisher() { return pimpl_->get_clock_publisher(); @@ -1324,4 +1462,140 @@ size_t Player::get_number_of_registered_on_play_msg_post_callbacks() return pimpl_->get_number_of_registered_on_play_msg_post_callbacks(); } +PlayerImpl::PlayerClient::PlayerClient( + std::shared_ptr cli, + std::string service_name, + std::string service_event_type, + const rclcpp::Logger & logger) +: client_(std::move(cli)), + service_name_(service_name), + logger_(logger) +{ + ts_lib_ = rclcpp::get_typesupport_library( + service_event_type, "rosidl_typesupport_cpp"); + + ts_ = rclcpp::get_typesupport_handle( + service_event_type, + "rosidl_typesupport_cpp", + *ts_lib_); + + auto ts_handle = get_message_typesupport_handle( + ts_, + rosidl_typesupport_introspection_cpp::typesupport_identifier); + + message_members_ = + reinterpret_cast( + ts_handle->data); +} + +bool PlayerImpl::PlayerClient::is_include_request_message(const rclcpp::SerializedMessage & message) +{ + auto type = get_msg_event_type(message); + + // Ignore response message + if (type == service_msgs::msg::ServiceEventInfo::RESPONSE_SENT || + type == service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED) + { + return false; + } + + // Ignore metadata message + if (type == service_msgs::msg::ServiceEventInfo::REQUEST_SENT && + client_side_type_ == introspection_type::METADATA) + { + return false; + } + if (type == service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED && + service_side_type_ == introspection_type::METADATA) + { + return false; + } + + if (client_side_type_ == introspection_type::UNKNOW && + type == service_msgs::msg::ServiceEventInfo::REQUEST_SENT) + { + if (message.size() <= rosbag2_cpp::get_serialization_size_for_service_metadata_event()) { + client_side_type_ = introspection_type::METADATA; + RCLCPP_WARN( + logger_, + "The configuration of introspection for '%s' client is metadata !", + service_name_.c_str()); + return false; + } else { + client_side_type_ = introspection_type::CONTENTS; + } + } + if (service_side_type_ == introspection_type::UNKNOW && + type == service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) + { + if (message.size() <= rosbag2_cpp::get_serialization_size_for_service_metadata_event()) { + service_side_type_ = introspection_type::METADATA; + RCLCPP_WARN( + logger_, + "The configuration of introspection for '%s' service is metadata !", + service_name_.c_str()); + return false; + } else { + service_side_type_ = introspection_type::CONTENTS; + } + } + + // If there are request send info and request receive info, only send request send info. + if (client_side_type_ == introspection_type::CONTENTS && + service_side_type_ == introspection_type::CONTENTS && + type == service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) + { + return false; + } + + return true; +} + +void PlayerImpl::PlayerClient::async_send_request(const rclcpp::SerializedMessage & message) +{ + void * ros_message = new uint8_t[message_members_->size_of_]; + message_members_->init_function( + ros_message, rosidl_runtime_cpp::MessageInitialization::ZERO); + + int ret = rmw_deserialize(&message.get_rcl_serialized_message(), ts_, ros_message); + if (ret == RMW_RET_OK) { + if (client_->service_is_ready()) { + // members_[0]: info, members_[1]: request, members_[2]: response + auto request_offset = message_members_->members_[1].offset_; + auto request_addr = reinterpret_cast(ros_message) + request_offset; + client_->async_send_request( + reinterpret_cast(*reinterpret_cast(request_addr))); + } else { + RCLCPP_ERROR( + logger_, + "'%s' service isn't ready !", + service_name_.c_str()); + } + } else { + throw std::runtime_error( + "Failed to deserialize service event message for " + service_name_ + " !"); + } + + message_members_->fini_function(ros_message); + delete[] static_cast(ros_message); +} + +uint8_t PlayerImpl::PlayerClient::get_msg_event_type(const rclcpp::SerializedMessage & message) +{ + auto msg = service_msgs::msg::ServiceEventInfo(); + + const rosidl_message_type_support_t * type_support_info = + rosidl_typesupport_cpp:: + get_message_type_support_handle(); + + auto ret = rmw_deserialize( + &message.get_rcl_serialized_message(), + type_support_info, + reinterpret_cast(&msg)); + if (ret != RMW_RET_OK) { + throw std::runtime_error("It failed to deserialize message !"); + } + + return msg.event_type; +} } // namespace rosbag2_transport diff --git a/rosbag2_transport/test/rosbag2_transport/mock_player.hpp b/rosbag2_transport/test/rosbag2_transport/mock_player.hpp index 7a0143a1c9..8d5d89a578 100644 --- a/rosbag2_transport/test/rosbag2_transport/mock_player.hpp +++ b/rosbag2_transport/test/rosbag2_transport/mock_player.hpp @@ -48,6 +48,18 @@ class MockPlayer : public rosbag2_transport::Player return pub_list; } + std::vector get_list_of_clients() + { + std::vector cli_list; + for (const auto & client : get_clients()) { + cli_list.push_back( + static_cast( + client.second.get())); + } + + return cli_list; + } + using rosbag2_transport::Player::wait_for_playback_to_start; size_t get_number_of_registered_pre_callbacks() diff --git a/rosbag2_transport/test/rosbag2_transport/mock_sequential_reader.hpp b/rosbag2_transport/test/rosbag2_transport/mock_sequential_reader.hpp index 46ac168c01..6551f98929 100644 --- a/rosbag2_transport/test/rosbag2_transport/mock_sequential_reader.hpp +++ b/rosbag2_transport/test/rosbag2_transport/mock_sequential_reader.hpp @@ -43,14 +43,24 @@ class MockSequentialReader : public rosbag2_cpp::reader_interfaces::BaseReaderIn bool has_next() override { - if (filter_.topics.empty()) { + if (filter_.topics.empty() && filter_.services.empty()) { return num_read_ < messages_.size(); } while (num_read_ < messages_.size()) { - for (const auto & filter_topic : filter_.topics) { - if (!messages_[num_read_]->topic_name.compare(filter_topic)) { - return true; + if (!filter_.topics.empty()) { + for (const auto & filter_topic : filter_.topics) { + if (!messages_[num_read_]->topic_name.compare(filter_topic)) { + return true; + } + } + } + + if (!filter_.services.empty()) { + for (const auto & filter_service : filter_.services) { + if (!messages_[num_read_]->topic_name.compare(filter_service)) { + return true; + } } } num_read_++; diff --git a/rosbag2_transport/test/rosbag2_transport/rosbag2_play_test_fixture.hpp b/rosbag2_transport/test/rosbag2_transport/rosbag2_play_test_fixture.hpp index c68ab7b01f..7b429e7140 100644 --- a/rosbag2_transport/test/rosbag2_transport/rosbag2_play_test_fixture.hpp +++ b/rosbag2_transport/test/rosbag2_transport/rosbag2_play_test_fixture.hpp @@ -18,6 +18,7 @@ #include #include "rosbag2_test_common/subscription_manager.hpp" +#include "rosbag2_test_common/service_manager.hpp" #include "rosbag2_transport_test_fixture.hpp" class RosBag2PlayTestFixture : public Rosbag2TransportTestFixture @@ -28,6 +29,7 @@ class RosBag2PlayTestFixture : public Rosbag2TransportTestFixture { rclcpp::init(0, nullptr); sub_ = std::make_shared(); + srv_ = std::make_shared(); } ~RosBag2PlayTestFixture() override @@ -36,6 +38,7 @@ class RosBag2PlayTestFixture : public Rosbag2TransportTestFixture } std::shared_ptr sub_; + std::shared_ptr srv_; }; #endif // ROSBAG2_TRANSPORT__ROSBAG2_PLAY_TEST_FIXTURE_HPP_ diff --git a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp index 3e666b56ad..8b6f1c2542 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp @@ -29,6 +29,7 @@ using namespace ::testing; // NOLINT using namespace rosbag2_transport; // NOLINT using namespace rosbag2_test_common; // NOLINT +using namespace std::chrono_literals; // NOLINT TEST_F(RosBag2PlayTestFixture, burst_with_false_preconditions) { auto primitive_message = get_messages_basic_types()[0]; @@ -365,3 +366,54 @@ TEST_F(RosBag2PlayTestFixture, burst_bursting_only_filtered_topics) { // All we care is that any messages arrived EXPECT_THAT(replayed_topic2, SizeIs(Eq(EXPECTED_BURST_COUNT))); } + +TEST_F(RosBag2PlayTestFixture, burst_bursting_only_filtered_services) { + auto services_types = std::vector{ + {"test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", "", ""}, + {"test_service2/_service_event", "test_msgs/srv/BasicTypes_Event", "", "", ""}, + }; + std::vector> messages = + { + serialize_test_message( + "test_service1/_service_event", 500, get_service_event_message_basic_types()[0]), + serialize_test_message( + "test_service2/_service_event", 600, get_service_event_message_basic_types()[0]), + serialize_test_message( + "test_service1/_service_event", 400, get_service_event_message_basic_types()[1]), + serialize_test_message( + "test_service2/_service_event", 500, get_service_event_message_basic_types()[1]) + }; + + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + + srv_->setup_service("test_service1", service1_receive_requests); + srv_->setup_service("test_service2", service2_receive_requests); + + srv_->run_services(); + + // Filter allows test_service2, blocks test_service1 + play_options_.services_to_filter = {"test_service2/_service_event"}; + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, services_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + auto player = std::make_shared(std::move(reader), storage_options_, play_options_); + + player->pause(); + ASSERT_TRUE(player->is_paused()); + + auto player_future = std::async(std::launch::async, [&player]() -> void {player->play();}); + ASSERT_TRUE(player->is_paused()); + + const size_t EXPECTED_BURST_COUNT = 2; + ASSERT_EQ(player->burst(EXPECTED_BURST_COUNT), EXPECTED_BURST_COUNT); + + ASSERT_TRUE(player->is_paused()); + player->resume(); + player_future.get(); + + std::this_thread::sleep_for(200ms); + + EXPECT_EQ(service1_receive_requests.size(), 0); + EXPECT_EQ(service2_receive_requests.size(), 2); +} diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 7b33abbcdc..3df6ee3b17 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -25,12 +25,14 @@ #include "rclcpp/rclcpp.hpp" #include "rosbag2_test_common/subscription_manager.hpp" +#include "rosbag2_test_common/service_manager.hpp" #include "rosbag2_transport/player.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 "rosbag2_transport/qos.hpp" @@ -109,6 +111,104 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics) ElementsAre(40.0f, 2.0f, 0.0f))))); } +TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_services) +{ + auto services_types = std::vector{ + {"test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", "", ""}, + {"test_service2/_service_event", "test_msgs/srv/BasicTypes_Event", "", "", ""}, + }; + std::vector> messages = + { + serialize_test_message( + "test_service1/_service_event", 500, get_service_event_message_basic_types()[0]), + serialize_test_message( + "test_service2/_service_event", 600, get_service_event_message_basic_types()[0]), + serialize_test_message( + "test_service1/_service_event", 400, get_service_event_message_basic_types()[1]), + serialize_test_message( + "test_service2/_service_event", 500, get_service_event_message_basic_types()[1]) + }; + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, services_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + + srv_->setup_service("test_service1", service1_receive_requests); + srv_->setup_service("test_service2", service2_receive_requests); + + srv_->run_services(); + + auto player = std::make_shared( + std::move( + reader), storage_options_, play_options_); + player->play(); + + std::this_thread::sleep_for(300ms); + EXPECT_EQ(service1_receive_requests.size(), 2); + EXPECT_EQ(service2_receive_requests.size(), 2); +} + +TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_topics_and_services) +{ + auto topic_msg = get_messages_basic_types()[0]; + topic_msg->int64_value = 1111; + + auto services_types = std::vector{ + {"topic1", "test_msgs/BasicTypes", "", "", ""}, + {"test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", "", ""}, + }; + std::vector> messages = + { + serialize_test_message( + "test_service1/_service_event", 500, get_service_event_message_basic_types()[0]), + serialize_test_message( + "topic1", 600, topic_msg), + serialize_test_message( + "test_service1/_service_event", 550, get_service_event_message_basic_types()[1]), + serialize_test_message( + "topic1", 400, topic_msg), + }; + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, services_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + std::vector> service_receive_requests; + srv_->setup_service("test_service1", service_receive_requests); + srv_->run_services(); + + sub_->add_subscription("/topic1", 2); + auto await_received_messages = sub_->spin_subscriptions(); + + auto player = std::make_shared( + std::move( + reader), storage_options_, play_options_); + player->play(); + + await_received_messages.get(); + + auto replayed_topic_msg = sub_->get_received_messages( + "/topic1"); + EXPECT_THAT(replayed_topic_msg, SizeIs(Ge(2u))); + EXPECT_THAT( + replayed_topic_msg, + Each(Pointee(Field(&test_msgs::msg::BasicTypes::int64_value, 1111)))); + + std::this_thread::sleep_for(200ms); + ASSERT_EQ(service_receive_requests.size(), 2); + for (size_t i = 0; i < 2; i++) { + EXPECT_EQ( + service_receive_requests[i]->int32_value, + get_service_event_message_basic_types()[i]->request[0].int32_value); + EXPECT_EQ( + service_receive_requests[i]->int64_value, + get_service_event_message_basic_types()[i]->request[0].int64_value); + } +} + TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics_with_unknown_type) { auto primitive_message1 = get_messages_basic_types()[0]; @@ -302,6 +402,266 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics) } } +TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_services) +{ + auto services_types = std::vector{ + {"test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", "", ""}, + {"test_service2/_service_event", "test_msgs/srv/BasicTypes_Event", "", "", ""}, + }; + std::vector> messages = + { + serialize_test_message( + "test_service1/_service_event", 500, get_service_event_message_basic_types()[0]), + serialize_test_message( + "test_service2/_service_event", 600, get_service_event_message_basic_types()[0]), + serialize_test_message( + "test_service1/_service_event", 400, get_service_event_message_basic_types()[1]), + serialize_test_message( + "test_service2/_service_event", 500, get_service_event_message_basic_types()[1]) + }; + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, services_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + // Filter allows /test_service2, blocks /test_service1 + { + play_options_.services_to_filter = {"test_service2/_service_event"}; + + srv_.reset(); + srv_ = std::make_shared(); + + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + srv_->setup_service("test_service1", service1_receive_requests); + srv_->setup_service("test_service2", service2_receive_requests); + + srv_->run_services(); + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, services_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + auto player = std::make_shared( + std::move( + reader), storage_options_, play_options_); + player->play(); + + std::this_thread::sleep_for(250ms); + EXPECT_EQ(service1_receive_requests.size(), 0); + EXPECT_EQ(service2_receive_requests.size(), 2); + } + + // Filter allows /test_service1, blocks /test_service2 + { + play_options_.services_to_filter = {"test_service1/_service_event"}; + + srv_.reset(); + srv_ = std::make_shared(); + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + srv_->setup_service("test_service1", service1_receive_requests); + srv_->setup_service("test_service2", service2_receive_requests); + + srv_->run_services(); + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, services_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + auto player = std::make_shared( + std::move( + reader), storage_options_, play_options_); + player->play(); + + std::this_thread::sleep_for(250ms); + EXPECT_EQ(service1_receive_requests.size(), 2); + EXPECT_EQ(service2_receive_requests.size(), 0); + } + + // No filter, receive both services + { + play_options_.services_to_filter.clear(); + + srv_.reset(); + srv_ = std::make_shared(); + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + srv_->setup_service("test_service1", service1_receive_requests); + srv_->setup_service("test_service2", service2_receive_requests); + + srv_->run_services(); + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, services_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + auto player = std::make_shared( + std::move( + reader), storage_options_, play_options_); + player->play(); + + std::this_thread::sleep_for(250ms); + EXPECT_EQ(service1_receive_requests.size(), 2); + EXPECT_EQ(service2_receive_requests.size(), 2); + } +} + +TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics_and_services) +{ + auto all_types = std::vector{ + {"topic1", "test_msgs/BasicTypes", "", "", ""}, + {"topic2", "test_msgs/BasicTypes", "", "", ""}, + {"test_service1/_service_event", "test_msgs/srv/BasicTypes_Event", "", "", ""}, + {"test_service2/_service_event", "test_msgs/srv/BasicTypes_Event", "", "", ""}, + }; + + std::vector> messages = + { + serialize_test_message("topic1", 500, get_messages_basic_types()[0]), + serialize_test_message( + "test_service1/_service_event", + 520, + get_service_event_message_basic_types()[0]), + serialize_test_message("topic2", 520, get_messages_basic_types()[0]), + serialize_test_message( + "test_service2/_service_event", + 550, + get_service_event_message_basic_types()[0]), + }; + + // Filter allows all topics, blocks service test_service2 + { + play_options_.topics_to_filter = {"topic1", "topic2"}; + play_options_.services_to_filter = {"test_service1/_service_event"}; + + sub_.reset(); + sub_ = std::make_shared(); + sub_->add_subscription("/topic1", 1); + sub_->add_subscription("/topic2", 1); + + srv_.reset(); + srv_ = std::make_shared(); + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + srv_->setup_service("test_service1", service1_receive_requests); + srv_->setup_service("test_service2", service2_receive_requests); + srv_->run_services(); + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, all_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + auto await_received_messages = sub_->spin_subscriptions(); + + auto player = std::make_shared( + std::move( + reader), storage_options_, play_options_); + player->play(); + + await_received_messages.get(); + std::this_thread::sleep_for(300ms); + + // Filter allow all topics + auto replayed_topic1 = sub_->get_received_messages("/topic1"); + EXPECT_THAT(replayed_topic1, SizeIs(1u)); + auto replayed_topic2 = sub_->get_received_messages("/topic2"); + EXPECT_THAT(replayed_topic2, SizeIs(1u)); + + // Filter allow test_service1, block test_service2 + EXPECT_EQ(service1_receive_requests.size(), 1); + EXPECT_EQ(service2_receive_requests.size(), 0); + } + + // Filter allows all services, blocks topic2 + { + play_options_.topics_to_filter = {"topic1"}; + play_options_.services_to_filter = { + "test_service1/_service_event", "test_service2/_service_event"}; + + sub_.reset(); + sub_ = std::make_shared(); + sub_->add_subscription("/topic1", 1); + sub_->add_subscription("/topic2", 0); + + srv_.reset(); + srv_ = std::make_shared(); + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + srv_->setup_service("test_service1", service1_receive_requests); + srv_->setup_service("test_service2", service2_receive_requests); + srv_->run_services(); + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, all_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + auto await_received_messages = sub_->spin_subscriptions(); + + auto player = std::make_shared( + std::move( + reader), storage_options_, play_options_); + player->play(); + + await_received_messages.get(); + std::this_thread::sleep_for(200ms); + + // Filter allow topic2, block topic1 + auto replayed_topic1 = sub_->get_received_messages("/topic1"); + EXPECT_THAT(replayed_topic1, SizeIs(1u)); + auto replayed_topic2 = sub_->get_received_messages("/topic2"); + EXPECT_THAT(replayed_topic2, SizeIs(0u)); + + // Filter allow all services + EXPECT_EQ(service1_receive_requests.size(), 1); + EXPECT_EQ(service2_receive_requests.size(), 1); + } + + // Filter allows all services and topics + { + play_options_.topics_to_filter = {"topic1", "topic2"}; + play_options_.services_to_filter = { + "test_service1/_service_event", "test_service2/_service_event"}; + + sub_.reset(); + sub_ = std::make_shared(); + sub_->add_subscription("/topic1", 1); + sub_->add_subscription("/topic2", 1); + + srv_.reset(); + srv_ = std::make_shared(); + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + srv_->setup_service("test_service1", service1_receive_requests); + srv_->setup_service("test_service2", service2_receive_requests); + srv_->run_services(); + + auto prepared_mock_reader = std::make_unique(); + prepared_mock_reader->prepare(messages, all_types); + auto reader = std::make_unique(std::move(prepared_mock_reader)); + + auto await_received_messages = sub_->spin_subscriptions(); + + auto player = std::make_shared( + std::move( + reader), storage_options_, play_options_); + player->play(); + + await_received_messages.get(); + std::this_thread::sleep_for(200ms); + + // Filter allow all topics + auto replayed_topic1 = sub_->get_received_messages("/topic1"); + EXPECT_THAT(replayed_topic1, SizeIs(1u)); + auto replayed_topic2 = sub_->get_received_messages("/topic2"); + EXPECT_THAT(replayed_topic2, SizeIs(1u)); + + // Filter allow all services + EXPECT_EQ(service1_receive_requests.size(), 1); + EXPECT_EQ(service2_receive_requests.size(), 1); + } +} + TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics_with_unknown_type) { auto primitive_message1 = get_messages_basic_types()[0]; diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp index c35ce0e1b6..1358043059 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp @@ -69,7 +69,7 @@ TEST_F(RosBag2PlayTestFixture, play_bag_file_twice) { auto await_received_messages = sub_->spin_subscriptions(); rosbag2_transport::PlayOptions play_options = { - read_ahead_queue_size, "", rate, {}, {}, {}, {}, loop_playback, {}, + read_ahead_queue_size, "", rate, {}, {}, {}, {}, {}, {}, loop_playback, {}, clock_publish_frequency, clock_publish_per_topic, clock_trigger_topics, delay}; auto player = std::make_shared( std::move( @@ -131,7 +131,7 @@ TEST_F(RosBag2PlayTestFixture, messages_played_in_loop) { auto await_received_messages = sub_->spin_subscriptions(); rosbag2_transport::PlayOptions play_options{read_ahead_queue_size, "", rate, {}, {}, - {}, {}, loop_playback, {}, clock_publish_frequency, clock_publish_per_topic, + {}, {}, {}, {}, loop_playback, {}, clock_publish_frequency, clock_publish_per_topic, clock_trigger_topics, delay}; auto player = std::make_shared( std::move( diff --git a/rosbag2_transport/test/rosbag2_transport/test_record.cpp b/rosbag2_transport/test/rosbag2_transport/test_record.cpp index fc1a7ce220..59829f6754 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record.cpp @@ -23,7 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "rosbag2_test_common/publication_manager.hpp" -#include "rosbag2_test_common/service_client_manager.hpp" +#include "rosbag2_test_common/client_manager.hpp" #include "rosbag2_test_common/wait_for.hpp" #include "rosbag2_transport/recorder.hpp" diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp index a5b3933dba..be76ac4ce9 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_all.cpp @@ -26,7 +26,7 @@ #include "rosbag2_test_common/publication_manager.hpp" #include "rosbag2_test_common/wait_for.hpp" -#include "rosbag2_test_common/service_client_manager.hpp" +#include "rosbag2_test_common/client_manager.hpp" #include "rosbag2_transport/recorder.hpp" @@ -91,12 +91,12 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_topics_are TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_services_are_recorded) { - auto service_client_manager_1 = - std::make_shared>( + auto client_manager_1 = + std::make_shared>( "test_service_1"); - auto service_client_manager_2 = - std::make_shared>( + auto client_manager_2 = + std::make_shared>( "test_service_2"); rosbag2_transport::RecordOptions record_options = @@ -107,14 +107,14 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_services_a start_async_spin(recorder); - ASSERT_TRUE(service_client_manager_1->check_service_ready()); + ASSERT_TRUE(client_manager_1->check_service_ready()); - ASSERT_TRUE(service_client_manager_2->check_service_ready()); + ASSERT_TRUE(client_manager_2->check_service_ready()); // By default, only client introspection is enable. // For one request, service event topic get 2 messages. - ASSERT_TRUE(service_client_manager_1->send_request()); - ASSERT_TRUE(service_client_manager_2->send_request()); + ASSERT_TRUE(client_manager_1->send_request()); + ASSERT_TRUE(client_manager_2->send_request()); std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -129,8 +129,8 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_multiple_services_a TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_are_recorded) { - auto service_client_manager_1 = - std::make_shared>( + auto client_manager_1 = + std::make_shared>( "test_service"); auto string_message = get_messages_strings()[0]; @@ -150,13 +150,13 @@ TEST_F(RecordIntegrationTestFixture, published_messages_from_topic_and_service_a ASSERT_TRUE(pub_manager.wait_for_matched(string_topic.c_str())); - ASSERT_TRUE(service_client_manager_1->check_service_ready()); + ASSERT_TRUE(client_manager_1->check_service_ready()); pub_manager.run_publishers(); // By default, only client introspection is enable. // For one request, service event topic get 2 messages. - ASSERT_TRUE(service_client_manager_1->send_request()); + ASSERT_TRUE(client_manager_1->send_request()); std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp index 52c21b1060..7af1a6a815 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_regex.cpp @@ -24,7 +24,7 @@ #include "rosbag2_test_common/publication_manager.hpp" #include "rosbag2_test_common/wait_for.hpp" -#include "rosbag2_test_common/service_client_manager.hpp" +#include "rosbag2_test_common/client_manager.hpp" #include "rosbag2_transport/recorder.hpp" @@ -201,19 +201,19 @@ TEST_F(RecordIntegrationTestFixture, regex_and_exclude_service_recording) record_options.exclude_services = services_regex_to_exclude; auto service_manager_v1 = - std::make_shared>(v1); + std::make_shared>(v1); auto service_manager_v2 = - std::make_shared>(v2); + std::make_shared>(v2); auto service_manager_e1 = - std::make_shared>(e1); + std::make_shared>(e1); auto service_manager_b1 = - std::make_shared>(b1); + std::make_shared>(b1); auto service_manager_b2 = - std::make_shared>(b2); + std::make_shared>(b2); auto recorder = std::make_shared( std::move(writer_), storage_options_, record_options); From d5ee31697f29efed4a1995296b950bb1587d2541 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 6 Sep 2023 15:53:03 +0800 Subject: [PATCH 3/8] Update test codes for burst and play Signed-off-by: Barry Xu --- .../test/rosbag2_transport/test_burst.cpp | 32 +++++++++++++++++++ .../test/rosbag2_transport/test_play.cpp | 31 ++++++++++++++++++ 2 files changed, 63 insertions(+) diff --git a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp index 8b6f1c2542..5f990aac02 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp @@ -25,12 +25,44 @@ #include "test_msgs/message_fixtures.hpp" #include "test_msgs/msg/arrays.hpp" #include "test_msgs/msg/basic_types.hpp" +#include "test_msgs/srv/basic_types.hpp" using namespace ::testing; // NOLINT using namespace rosbag2_transport; // NOLINT using namespace rosbag2_test_common; // NOLINT using namespace std::chrono_literals; // NOLINT +namespace +{ +static inline std::vector +get_service_event_message_basic_types() +{ + std::vector messages; + + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT; + test_msgs::srv::BasicTypes_Request request; + request.int32_value = 123; + request.int64_value = 456; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT; + test_msgs::srv::BasicTypes_Request request; + request.int32_value = 456; + request.int64_value = 789; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + return messages; +} +} // namespace + TEST_F(RosBag2PlayTestFixture, burst_with_false_preconditions) { auto primitive_message = get_messages_basic_types()[0]; primitive_message->int32_value = 42; diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 3df6ee3b17..bb4470f002 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play.cpp @@ -45,6 +45,37 @@ using namespace rosbag2_transport; // NOLINT using namespace std::chrono_literals; // NOLINT using namespace rosbag2_test_common; // NOLINT +namespace +{ +static inline std::vector +get_service_event_message_basic_types() +{ + std::vector messages; + + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT; + test_msgs::srv::BasicTypes_Request request; + request.int32_value = 123; + request.int64_value = 456; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT; + test_msgs::srv::BasicTypes_Request request; + request.int32_value = 456; + request.int64_value = 789; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + return messages; +} +} // namespace + TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics) { auto primitive_message1 = get_messages_basic_types()[0]; From b873de8e944a6e17061fc79aca2b3793a5da7ece Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Thu, 7 Sep 2023 13:36:03 +0800 Subject: [PATCH 4/8] Optimize codes for more readable Signed-off-by: Barry Xu --- .../src/rosbag2_transport/player.cpp | 55 +++++++++---------- 1 file changed, 25 insertions(+), 30 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index cfd660eb11..1734eb7b09 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -509,7 +509,7 @@ bool PlayerImpl::play() } for (auto pub : senders_) { try { - if (pub.second.index() == 0) { // publisher + if (std::holds_alternative(pub.second)) { if (!std::get(pub.second) ->generic_publisher()->wait_for_all_acked(timeout)) { @@ -1048,7 +1048,7 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr auto sender_iter = senders_.find(message->topic_name); if (sender_iter != senders_.end()) { // For sending requests, ignore service event messages that do not contain request information. - if (sender_iter->second.index() == 1 && + if (std::holds_alternative(sender_iter->second) && !std::get(sender_iter->second) ->is_include_request_message(rclcpp::SerializedMessage(*message->serialized_data))) { @@ -1064,34 +1064,29 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr } } - switch (sender_iter->second.index()) { - case 0: // publisher - { - try { - std::get(sender_iter->second) - ->publish(rclcpp::SerializedMessage(*message->serialized_data)); - message_published = true; - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - owner_->get_logger(), "Failed to publish message on '" << message->topic_name << - "' topic. \nError: " << e.what()); - } - } - break; - case 1: // Client - { - try { - std::get(sender_iter->second) - ->async_send_request(rclcpp::SerializedMessage(*message->serialized_data)); - message_published = true; - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM( - owner_->get_logger(), "Failed to send request on '" << - rosbag2_cpp::service_event_topic_name_to_service_name(message->topic_name) << - "' service. \nError: " << e.what()); - } - } - break; + if (std::holds_alternative(sender_iter->second)) { + try { + std::get(sender_iter->second) + ->publish(rclcpp::SerializedMessage(*message->serialized_data)); + message_published = true; + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + owner_->get_logger(), "Failed to publish message on '" << message->topic_name << + "' topic. \nError: " << e.what()); + } + } else if (std::holds_alternative(sender_iter->second)) { + try { + std::get(sender_iter->second) + ->async_send_request(rclcpp::SerializedMessage(*message->serialized_data)); + message_published = true; + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM( + owner_->get_logger(), "Failed to send request on '" << + rosbag2_cpp::service_event_topic_name_to_service_name(message->topic_name) << + "' service. \nError: " << e.what()); + } + } else { + RCLCPP_ERROR_STREAM(owner_->get_logger(), "Unknown type of sender !"); } // Calling on play message post-callbacks From 1d2aa1ee346be4c7c0fa66639fcaea66fedbf9ea Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Tue, 12 Sep 2023 10:28:31 +0800 Subject: [PATCH 5/8] Address reivew comments Signed-off-by: Barry Xu --- docs/message_definition_encoding.md | 2 +- ros2bag/ros2bag/api/__init__.py | 23 +++++++++++++- ros2bag/ros2bag/verb/burst.py | 8 ++--- ros2bag/ros2bag/verb/play.py | 31 +++++++++++++------ ros2bag/ros2bag/verb/record.py | 16 +++++----- rosbag2_cpp/include/rosbag2_cpp/info.hpp | 6 ++-- .../include/rosbag2_cpp/service_utils.hpp | 20 +++++++++--- rosbag2_cpp/src/rosbag2_cpp/info.cpp | 12 +++---- rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 12 ++++--- .../rosbag2_cpp/writers/sequential_writer.cpp | 4 +-- .../test/rosbag2_cpp/test_service_utils.cpp | 7 +++-- rosbag2_py/src/rosbag2_py/_info.cpp | 2 +- .../src/rosbag2_py/format_service_info.cpp | 5 +-- .../src/rosbag2_py/format_service_info.hpp | 2 +- .../src/rosbag2_transport/player.cpp | 12 +++++-- 15 files changed, 107 insertions(+), 55 deletions(-) diff --git a/docs/message_definition_encoding.md b/docs/message_definition_encoding.md index 2c9d5db6ff..7d6f5a7554 100644 --- a/docs/message_definition_encoding.md +++ b/docs/message_definition_encoding.md @@ -16,7 +16,7 @@ This set of definitions with all field types recursively included can be called ## `ros2msg` encoding -This encoding consists of definitions in [.msg](https://docs.ros.org/en/rolling/Concepts/About-ROS-Interfaces.html#message-description-specification) and [.srv](https://docs.ros.org/en/rolling/Concepts/Basic/About-Interfaces.html#services) 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: diff --git a/ros2bag/ros2bag/api/__init__.py b/ros2bag/ros2bag/api/__init__.py index 0c9a192ed2..5fdcb55a8e 100644 --- a/ros2bag/ros2bag/api/__init__.py +++ b/ros2bag/ros2bag/api/__init__.py @@ -55,7 +55,15 @@ def _split_lines(self, text, width): def print_error(string: str) -> str: - return '[ERROR] [ros2bag]: {}'.format(string) + return _print_base('ERROR', string) + + +def print_warn(string: str) -> str: + return _print_base('WARN', string) + + +def _print_base(print_type: str, string: str) -> str: + return '[{}] [ros2bag]: {}'.format(print_type, string) def dict_to_duration(time_dict: Optional[Dict[str, int]]) -> Duration: @@ -200,3 +208,16 @@ def add_writer_storage_plugin_extensions(parser: ArgumentParser) -> None: 'Settings in this profile can still be overridden by other explicit options ' 'and --storage-config-file. Profiles:\n' + '\n'.join([f'{preset[0]}: {preset[1]}' for preset in preset_profiles])) + + +def convert_service_to_service_event_topic(services): + services_event_topics = [] + + if not services: + return services_event_topics + + for service in services: + name = '/' + service if service[0] != '/' else service + services_event_topics.append(name + '/_service_event') + + return services_event_topics diff --git a/ros2bag/ros2bag/verb/burst.py b/ros2bag/ros2bag/verb/burst.py index bbd9f8589c..26547adb53 100644 --- a/ros2bag/ros2bag/verb/burst.py +++ b/ros2bag/ros2bag/verb/burst.py @@ -18,6 +18,7 @@ from ros2bag.api import add_standard_reader_args from ros2bag.api import check_not_negative_int from ros2bag.api import check_positive_float +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.verb import VerbExtension @@ -95,12 +96,7 @@ def main(self, *, args): # noqa: D102 play_options.rate = 1.0 play_options.topics_to_filter = args.topics # Convert service name to service event topic name - services = [] - if args.services and len(args.services) != 0: - for s in args.services: - name = '/' + s if s[0] != '/' else s - services.append(name + '/_service_event') - play_options.services_to_filter = services + play_options.services_to_filter = convert_service_to_service_event_topic(args.services) play_options.topic_qos_profile_overrides = qos_profile_overrides play_options.loop = False play_options.topic_remapping_options = topic_remapping diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index d186137038..465389b76b 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -18,8 +18,10 @@ from ros2bag.api import add_standard_reader_args from ros2bag.api import check_not_negative_int from ros2bag.api import check_positive_float +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 print_warn from ros2bag.verb import VerbExtension from ros2cli.node import NODE_NAME_PREFIX from rosbag2_py import Player @@ -53,12 +55,17 @@ def add_arguments(self, parser, cli_name): # noqa: D102 help='Space-delimited list of topics to play.') parser.add_argument( '--services', type=str, default=[], nargs='+', - help='services to replay, separated by space. At least one service needs to be ' - 'specified.') + help='services to replay, separated by space. if none specified, all services will ' + 'be played.') parser.add_argument( '-e', '--regex', default='', help='filter topics by regular expression to replay, separated by space. If none ' 'specified, all topics will be replayed.') + parser.add_argument( + '-x', '--exclude', default='', + help='regular expressions to exclude topics from replay, separated by space. If none ' + 'specified, all topics will be replayed. This argument is deprecated and please ' + 'use --exclude-topics.') parser.add_argument( '--exclude-topics', default='', help='regular expressions to exclude topics from replay, separated by space. If none ' @@ -171,6 +178,10 @@ def main(self, *, args): # noqa: D102 except (InvalidQoSProfileException, ValueError) as e: return print_error(str(e)) + if args.exclude and args.exclude_topics: + return print_error(str('-x/--exclude and --exclude_topics cannot be used at the ' + 'same time.')) + storage_config_file = '' if args.storage_config_file: storage_config_file = args.storage_config_file.name @@ -192,15 +203,17 @@ def main(self, *, args): # noqa: D102 play_options.topics_to_filter = args.topics # Convert service name to service event topic name - services = [] - if args.services and len(args.services) != 0: - for s in args.services: - name = '/' + s if s[0] != '/' else s - services.append(name + '/_service_event') - play_options.services_to_filter = services + play_options.services_to_filter = convert_service_to_service_event_topic(args.services) play_options.regex_to_filter = args.regex - play_options.topics_regex_to_exclude = args.exclude_topics + + if args.exclude: + print(print_warn(str('-x/--exclude argument is deprecated. Please use ' + '--exclude-topics.'))) + play_options.topics_regex_to_exclude = args.exclude + else: + play_options.topics_regex_to_exclude = args.exclude_topics + if args.exclude_services: play_options.services_regex_to_exclude = args.exclude_services + '/_service_event' else: diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index dce1361dc7..3b3e0ed1da 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -18,6 +18,7 @@ from rclpy.qos import InvalidQoSProfileException from ros2bag.api import add_writer_storage_plugin_extensions +from ros2bag.api import convert_service_to_service_event_topic from ros2bag.api import convert_yaml_to_qos_profile from ros2bag.api import print_error from ros2bag.api import SplitLineFormatter @@ -183,11 +184,17 @@ 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 main(self, *, args): # noqa: D102 + def _check_necessary_argument(self, args): # One options out of --all, --all-topics, --all-services, --services, topics or --regex # must be used if not (args.all or args.all_topics or args.all_services or args.services or (args.topics and len(args.topics) > 0) or args.regex): + return False + return True + + def main(self, *, args): # noqa: D102 + + if not self._check_necessary_argument(args): return print_error('Must specify only one option out of --all, --all-topics, ' '--all-services, --services, topics and --regex') @@ -286,12 +293,7 @@ def main(self, *, args): # noqa: D102 record_options.all_services = args.all_services or args.all # Convert service name to service event topic name - services = [] - if args.services and len(args.services) != 0: - for s in args.services: - name = '/' + s if s[0] != '/' else s - services.append(name + '/_service_event') - record_options.services = services + record_options.services = convert_service_to_service_event_topic(args.services) recorder = Recorder() diff --git a/rosbag2_cpp/include/rosbag2_cpp/info.hpp b/rosbag2_cpp/include/rosbag2_cpp/info.hpp index 3d109d4b38..a251598ea5 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/info.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/info.hpp @@ -26,14 +26,14 @@ namespace rosbag2_cpp { -typedef struct +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; -} service_info; +} rosbag2_service_info_t; class ROSBAG2_CPP_PUBLIC Info { @@ -43,7 +43,7 @@ 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( + virtual std::vector> read_service_info( const std::string & uri, const std::string & storage_id = ""); }; diff --git a/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp index f6ddce86a2..188d18396c 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp @@ -17,15 +17,25 @@ #include +#include "rosbag2_cpp/visibility_control.hpp" + namespace rosbag2_cpp { -bool is_service_event_topic(const std::string & topic, const std::string & topic_type); +ROSBAG2_CPP_PUBLIC +bool +is_service_event_topic(const std::string & topic, const std::string & topic_type); -std::string service_event_topic_name_to_service_name(const std::string & topic_name); +ROSBAG2_CPP_PUBLIC +std::string +service_event_topic_name_to_service_name(const std::string & topic_name); -std::string service_event_topic_type_to_service_type(const std::string & topic_type); +ROSBAG2_CPP_PUBLIC +std::string +service_event_topic_type_to_service_type(const std::string & topic_type); -size_t get_serialization_size_for_service_metadata_event(); -} +ROSBAG2_CPP_PUBLIC +size_t +get_serialization_size_for_service_metadata_event(); +} // namespace rosbag2_cpp #endif // ROSBAG2_CPP__SERVICE_UTILS_HPP_ diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index c91a8a4a99..cf4faade5c 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "rosbag2_cpp/info.hpp" + #include #include #include @@ -22,7 +23,6 @@ #include "rmw/rmw.h" #include "rcpputils/filesystem_helper.hpp" -#include "rosbag2_cpp/info.hpp" #include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/logging.hpp" #include "rosbag2_storage/metadata_io.hpp" @@ -84,7 +84,7 @@ struct service_req_resp_info }; } // namespace -std::vector> Info::read_service_info( +std::vector> Info::read_service_info( const std::string & uri, const std::string & storage_id) { rosbag2_storage::StorageFactory factory; @@ -96,13 +96,13 @@ std::vector> Info::read_service_info( using service_analysis = std::unordered_map>; - std::unordered_map> all_service_info; + 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(); + 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; @@ -111,7 +111,7 @@ std::vector> Info::read_service_info( } } - std::vector> ret_service_info; + std::vector> ret_service_info; if (!all_service_info.empty()) { auto msg = service_msgs::msg::ServiceEventInfo(); diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp index e658db7b97..19cecf3fd5 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -46,11 +46,15 @@ bool is_service_event_topic(const std::string & topic, const std::string & topic return false; } - std::string end_topic_type = topic_type.substr( - topic_type.length() - std::strlen(service_event_topic_type_postfix)); + if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { + return false; + } - return end_topic_name == RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX && - end_topic_type == service_event_topic_type_postfix; + return (topic_type.compare( + topic_type.length() - std::strlen(service_event_topic_type_postfix), + std::strlen(service_event_topic_type_postfix), + service_event_topic_type_postfix) == 0) && + (end_topic_name == RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); } std::string service_event_topic_name_to_service_name(const std::string & topic_name) diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 77d7b04618..acb36eb893 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -200,9 +200,7 @@ void SequentialWriter::create_topic(const rosbag2_storage::TopicMetadata & topic 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 = topic_with_type.type.substr( - 0, - topic_with_type.type.length() - strlen("_Event")); + topic_type = service_event_topic_type_to_service_type(topic_with_type.type); } else { topic_type = topic_with_type.type; } diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp index b04c0e6afd..51420e20ac 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include "rosbag2_cpp/service_utils.hpp" @@ -26,7 +27,7 @@ class ServiceUtilsTest : public Test TEST_F(ServiceUtilsTest, check_is_service_event_topic) { - std::vector, bool>> all_test_data = + std::vector, bool>> all_test_data = { {{"/abc/_service_event", "package/srv/xyz_Event"}, true}, {{"/_service_event", "package/srv/xyz_Event"}, false}, @@ -37,8 +38,8 @@ TEST_F(ServiceUtilsTest, check_is_service_event_topic) for (const auto & test_data : all_test_data) { EXPECT_TRUE( - rosbag2_cpp::is_service_event_topic(test_data.first.first, test_data.first.second) == - test_data.second); + rosbag2_cpp::is_service_event_topic( + std::get<0>(test_data.first), std::get<1>(test_data.first)) == test_data.second); } } diff --git a/rosbag2_py/src/rosbag2_py/_info.cpp b/rosbag2_py/src/rosbag2_py/_info.cpp index 061e7317c3..f6e0f428a1 100644 --- a/rosbag2_py/src/rosbag2_py/_info.cpp +++ b/rosbag2_py/src/rosbag2_py/_info.cpp @@ -46,7 +46,7 @@ class Info { auto metadata_info = read_metadata(uri, storage_id); - std::vector> all_services_info; + std::vector> all_services_info; for (auto & file_info : metadata_info.files) { auto services_info = info_->read_service_info( uri + "/" + file_info.path, diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.cpp b/rosbag2_py/src/rosbag2_py/format_service_info.cpp index 07ee80edec..0c1366bee9 100644 --- a/rosbag2_py/src/rosbag2_py/format_service_info.cpp +++ b/rosbag2_py/src/rosbag2_py/format_service_info.cpp @@ -17,7 +17,8 @@ #include "format_service_info.hpp" std::string -format_service_info(std::vector> & service_info_list) +format_service_info( + std::vector> & service_info_list) { std::stringstream info_stream; int indentation_spaces = 21; @@ -29,7 +30,7 @@ format_service_info(std::vector> & se } auto print_service_info = - [&info_stream](const std::shared_ptr & si) -> void { + [&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 << " | "; diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.hpp b/rosbag2_py/src/rosbag2_py/format_service_info.hpp index f1aac050a4..7337e5be4e 100644 --- a/rosbag2_py/src/rosbag2_py/format_service_info.hpp +++ b/rosbag2_py/src/rosbag2_py/format_service_info.hpp @@ -22,6 +22,6 @@ #include "rosbag2_cpp/info.hpp" std::string format_service_info( - std::vector> & service_info); + std::vector> & service_info); #endif // ROSBAG2_PY__FORMAT_SERVICE_INFO_HPP_ diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 1734eb7b09..8631690fd5 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -1566,13 +1566,15 @@ void PlayerImpl::PlayerClient::async_send_request(const rclcpp::SerializedMessag "'%s' service isn't ready !", service_name_.c_str()); } - } else { - throw std::runtime_error( - "Failed to deserialize service event message for " + service_name_ + " !"); } message_members_->fini_function(ros_message); delete[] static_cast(ros_message); + + if (ret != RMW_RET_OK) { + throw std::runtime_error( + "Failed to deserialize service event message for " + service_name_ + " !"); + } } uint8_t PlayerImpl::PlayerClient::get_msg_event_type(const rclcpp::SerializedMessage & message) @@ -1582,6 +1584,10 @@ uint8_t PlayerImpl::PlayerClient::get_msg_event_type(const rclcpp::SerializedMes const rosidl_message_type_support_t * type_support_info = rosidl_typesupport_cpp:: get_message_type_support_handle(); + if (type_support_info == nullptr) { + throw std::runtime_error( + "It failed to get message type support handle of service event info !"); + } auto ret = rmw_deserialize( &message.get_rcl_serialized_message(), From 160104589b303d3bfc2e5df5319053b8a48c233c Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Wed, 20 Sep 2023 16:54:53 +0800 Subject: [PATCH 6/8] Fix test error after rebase Signed-off-by: Barry Xu --- .../test/rosbag2_transport/test_topic_filter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp index 05a3b681a8..c22d2047f1 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_topic_filter.cpp @@ -307,7 +307,7 @@ TEST_F(RegexFixture, do_not_print_warning_about_unknown_types_if_topic_is_not_se rosbag2_transport::RecordOptions record_options; // Select only one topic with name "/planning" via topic list record_options.topics = {"/planning"}; - record_options.all = false; + record_options.all_topics = false; rosbag2_transport::TopicFilter filter{record_options, nullptr, false}; testing::internal::CaptureStderr(); auto filtered_topics = filter.filter_topics(topics_and_types_); @@ -325,7 +325,7 @@ TEST_F(RegexFixture, do_not_print_warning_about_unknown_types_if_topic_is_not_se rosbag2_transport::RecordOptions record_options; // Select only one topic with name "/planning" via regex record_options.regex = "^/planning"; - record_options.all = false; + record_options.all_topics = false; rosbag2_transport::TopicFilter filter{record_options, nullptr, false}; testing::internal::CaptureStderr(); auto filtered_topics = filter.filter_topics(topics_and_types_); From 48fee06e1a224b099b8adb154ba676462592fb38 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 22 Sep 2023 08:54:30 +0800 Subject: [PATCH 7/8] The description is consistent with --topics option Signed-off-by: Barry Xu --- ros2bag/ros2bag/verb/play.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 465389b76b..b6632272d8 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -55,8 +55,7 @@ def add_arguments(self, parser, cli_name): # noqa: D102 help='Space-delimited list of topics to play.') parser.add_argument( '--services', type=str, default=[], nargs='+', - help='services to replay, separated by space. if none specified, all services will ' - 'be played.') + help='Space-delimited list of services to play.') parser.add_argument( '-e', '--regex', default='', help='filter topics by regular expression to replay, separated by space. If none ' From 2cea90dd7291c2ed63354c315f0db837cc909392 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Fri, 22 Sep 2023 13:06:59 +0800 Subject: [PATCH 8/8] Fix error from rebase action Signed-off-by: Barry Xu --- rosbag2_transport/src/rosbag2_transport/player.cpp | 10 +++++----- .../test/rosbag2_transport/mock_player.hpp | 6 +++--- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index 8631690fd5..18ace90615 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -189,9 +189,9 @@ class PlayerImpl /// \return Hashtable representing topic to publisher map excluding inner clock_publisher std::unordered_map> get_publishers(); - /// \brief Getter for clients corresponding to services + /// \brief Getter for clients corresponding to services /// \return Hashtable representing service name to client map - std::unordered_map> get_clients(); + std::unordered_map> get_clients(); /// \brief Getter for inner clock_publisher /// \return Shared pointer to the inner clock_publisher @@ -763,7 +763,7 @@ std::unordered_map(sender)) { topic_to_publisher_map[topic] = - std::get(sender)->generic_publisher(); + std::get(sender)->generic_publisher(); } } return topic_to_publisher_map; @@ -776,7 +776,7 @@ std::unordered_map(sender)) { topic_to_client_map[service_name] = - std::get(sender)->generic_client(); + std::get(sender)->generic_client(); } } return topic_to_client_map; @@ -1432,7 +1432,7 @@ std::unordered_map> Playe return pimpl_->get_publishers(); } -std::unordered_map> Player::get_clients() +std::unordered_map> Player::get_clients() { return pimpl_->get_clients(); } diff --git a/rosbag2_transport/test/rosbag2_transport/mock_player.hpp b/rosbag2_transport/test/rosbag2_transport/mock_player.hpp index 8d5d89a578..e2f6ea75f0 100644 --- a/rosbag2_transport/test/rosbag2_transport/mock_player.hpp +++ b/rosbag2_transport/test/rosbag2_transport/mock_player.hpp @@ -52,9 +52,9 @@ class MockPlayer : public rosbag2_transport::Player { std::vector cli_list; for (const auto & client : get_clients()) { - cli_list.push_back( - static_cast( - client.second.get())); + cli_list.push_back( + static_cast( + client.second.get())); } return cli_list;