From b02142a27fb91b2e4772eb5b9e37878668fe5a83 Mon Sep 17 00:00:00 2001 From: Barry Xu Date: Sun, 14 Apr 2024 05:11:29 +0800 Subject: [PATCH] Support service 2/2 --- rosbag2 service play (#1481) * Implement service play Signed-off-by: Barry Xu * Maintain the future queue of the request by timeout Signed-off-by: Barry Xu * Use get_message_typesupport_handle() instead of deprecated get_typesupport_handle() Signed-off-by: Barry Xu * Resolve the conflicts caused by the rebase Signed-off-by: Barry Xu * Address review comments from Fujita-san Signed-off-by: Barry Xu * Move codes to new function remove_complete_request_future Signed-off-by: Barry Xu * Move codes to new function remove_all_timeout_request_future() Signed-off-by: Barry Xu * Add warning log and lock protection Signed-off-by: Barry Xu * Optimize code in PlayerImpl::publish_message() Signed-off-by: Barry Xu * Changed the code logic for determining whether to use request data of client or service Signed-off-by: Barry Xu * Add warning log and tests Signed-off-by: Barry Xu * Correct the parameter descriptions and remove unnecessary code Signed-off-by: Barry Xu * Extend the parameters for the "play" command Add new parameters '--exclude-topics' and '--exclude-services'. Signed-off-by: Barry Xu * Move implmentation of struct client_id_hash to cpp Signed-off-by: Barry Xu * Address some minor review comments Signed-off-by: Barry Xu * Replace std::variant> Signed-off-by: Barry Xu * Added code for cleaning up pending requests Signed-off-by: Barry Xu * Avoid creating publisher or client for filtered topic Signed-off-by: Barry Xu * Update code on filtering message for mcap Signed-off-by: Barry Xu * Use explicit namespace for topic name and update tests Signed-off-by: Barry Xu * Update for the rebase Signed-off-by: Barry Xu * Simplify variable names Signed-off-by: Barry Xu * Adjust the default timeout and queue length for request future Signed-off-by: Barry Xu * Cleanup and optimization in mcap_storage.cpp - Rewrite topic_filter to make clean and concise implementation. - Also addressed multiple performance related issues in topic_filter. - Delete MCAP_COMPILE_DEFS ROSBAG2_STORAGE_MCAP_HAS_STORAGE_FILTER_TOPIC_REGEX compilation flag since it is become absolute on Rolling, and we are not using anymore the same version of the mcap storage plugin for older ROS 2 distros. Signed-off-by: Michael Orlov * Move mcap topic_filter tests to a separate file Signed-off-by: Michael Orlov * Add TestResetFilter and CanSelectWithTopicsListOnly to the mcap storage Signed-off-by: Michael Orlov * Add CanSelectWithServiceEventsListOnly test to the mcap storage Signed-off-by: Michael Orlov * Cleanups and renames in the sqlite_storage.cpp Signed-off-by: Michael Orlov * Move SQLite3 topic_filter tests to a separate file Signed-off-by: Michael Orlov * Bugfix in sqlite topic filters when service_events or topics lists empty - Also add new tests `CanSelectWithTopicsListOnly` and `CanSelectWithServiceEventsListOnly` to cover changes. Signed-off-by: Michael Orlov * Rename test_sqlite3_topic_filter.cpp for consistency Signed-off-by: Michael Orlov * Change API to use rcl_serialized_message_t to avoid 2 extra message copy - Changed API for `async_send_request(..)`, `is_include_request_message(..)` and `get_msg_event_type(..)`` to use `rcl_serialized_message_t` instead of the `rclcpp::SerializedMessage` to avoid 2 extra message copy. - Renamed `introspection_include_metadata_and_contents(..)` to the `service_event_include_metadata_and_contents(..)`. - Altered `check_service_event_include_metadata_and_contents` unit test to use real serialized service event for verification and comparison. Signed-off-by: Michael Orlov * Misc minor fixes from previous rounds of review and new findings (1) - Mostly renames, cleanup in style wording and optimization in constructors Signed-off-by: Michael Orlov * Address ament_flake8 warning in regards whitespaces in the blank lines Signed-off-by: Michael Orlov * Simplify logic in is_topic_selected_by_white_list_or_regex(..) Signed-off-by: Michael Orlov Co-authored with: Barry Xu Co-authored-by: Barry Xu * Check is_topic_in_black_list_or_exclude_regex(..) first Co-authored-by: Barry Xu Signed-off-by: Michael Orlov * Add test coverage for the cases when exclude lists overlap with include Signed-off-by: Michael Orlov * Bugfix for incorrectly including all services when regex is not empty - Also add test coverage with new tests FilterTopicsAndServicesWithRegexAndNonexistentTopicsList and FilterTopicsAndServicesWithRegexAndNonexistentServicesList Signed-off-by: Michael Orlov * Rename test_sqlite3_topic_filter in CMakeList.txt Signed-off-by: Michael Orlov * Update spin & termination and add service ready check before play Signed-off-by: Barry Xu * Fix the issue of published_messages_from_multiple_services_are_recorded failing randomly Increase the waiting time for the record to start spinning. Signed-off-by: Barry Xu * Make service_event_ts_lib as private member again - Motivation: The shared pointer to the service event type support library shall be a member variable to make sure that library loaded during the liveliness of the instance of this class, since we have a raw pointers to its inner members. Signed-off-by: Michael Orlov * Cleanup in PlayerServiceClient::async_send_request(ser_message) - Rewrite raw pointers arithmetic. Assumption that size_t represent size of the void* may not necessarily be true on all platforms. - Use shared pointer with custom deleter for deserialized message. - Assumption that we can take first element from bounded sequence by dereferencing raw pointer to the bounded sequence may not be necessarily be true and up to the underlying rmw and transport layer implementation. Use dedicated request_member.get_function(request_sequence_ptr, 0) function instead. - Add sanity checks `for service_event_members_` in PlayerServiceClient constructor. Signed-off-by: Michael Orlov * Refactoring. Do full deserialization and only once - Rationale: We can't rely on assumption that we can safely partially deserialize service event to the ServiceEventInfo structure. Signed-off-by: Michael Orlov * Specify service request from which introspection message and fix uncrustify errors Signed-off-by: Barry Xu * Revert uncrustify changes from previous commit. - Rationale: We are moving to the new version of the uncrustify in rolling and still haven't done it yet fully for the baseline. The discrepancy in style for other untouched files like logging.hpp shall be addressed in a separate PR. Signed-off-by: Michael Orlov * Rename service_request_from to the service_requests_source - Also rename enum class ServiceRequestFrom to the ServiceRequestsSource Signed-off-by: Michael Orlov * Add Player::wait_for_sent_service_requests_to_finish() API - We need this API to be able to write deterministic and non-flaky tests Signed-off-by: Michael Orlov * Mitigate potential issues related to the operations reordering on ARM Signed-off-by: Michael Orlov * Make tests play_service_requests_from_service(client) deterministic - Get rid of timeout inside tests and use newly added player->wait_for_sent_service_requests_to_finish(service_name) API. Signed-off-by: Michael Orlov * Misc findings and improvements 1 Signed-off-by: Michael Orlov * Rename get_services_clients() to the get_service_clients() Signed-off-by: Michael Orlov * Add a new CLI parameter "--publish-service-requests" for Player Also added a new option publish_service_requests to the PlayOptions. Note: By default rosbag2 player will publish service events only. Signed-off-by: Barry Xu Signed-off-by: Michael Orlov * Fix an issue on filtering topic when prepare publishers Co-authored-by: Barry Xu Signed-off-by: Michael Orlov * Cleanup in play_without_publish_service_requests - Long story short: Make it deterministic and run fast. Signed-off-by: Michael Orlov * Wrap code which can throw with try-catch in the publish_message(..) Signed-off-by: Michael Orlov * Delete some part of the code which became absolute and shall not be used Signed-off-by: Michael Orlov * Update test codes Signed-off-by: Barry Xu * Remove code on meaningless waiting for published_messages_from_multiple_services_are_recorded Signed-off-by: Barry Xu * Update the code following the rebase Signed-off-by: Barry Xu * Remove a unnecessary check and simplify the code Signed-off-by: Barry Xu * Cleanup in service replay related tests Signed-off-by: Michael Orlov * Regenerate Python stub files (.pyi) after altering python API in PR Signed-off-by: Michael Orlov * Increase the timeout of waiting for the service to be ready Signed-off-by: Barry Xu * Update the code for waiting on all futures of one service client Signed-off-by: Barry Xu * Cleanup API for wait_for_sent_requests_to_finish(..) Signed-off-by: Michael Orlov * Fixes for Windows CI build failure - Add storage factory object as member variable to the test fixture class. The storage factory object shall persist while returned storage object persist. Signed-off-by: Michael Orlov * Fix a typo Signed-off-by: Barry Xu * Increase timeout value to stabilize a test Signed-off-by: Barry Xu * Fix a bug in PlayerImpl::wait_for_sent_service_requests_to_finish Signed-off-by: Barry Xu * Disable test_burst for RTI DDS due to the failure with missing requests - The test `burst_bursting_only_filtered_services` fails only with rmw_connext_dds for unknown reasons and tends to be flaky. Sometimes we receive only one service request instead of 2. Signed-off-by: Michael Orlov * Revert "Disable test_burst for RTI DDS due to the failure with missing requests" This reverts commit aaaac7439781bb6522cd834d8870cab593c08d8e. Signed-off-by: Michael Orlov * Disable burst_bursting_only_filtered_services for rmw_connextdds - The test `burst_bursting_only_filtered_services` fails only with rmw_connext_dds for unknown reasons and tends to be flaky. Sometimes we receive only one service request instead of 2. Signed-off-by: Michael Orlov --------- Signed-off-by: Barry Xu Signed-off-by: Michael Orlov Co-authored-by: Michael Orlov --- ros2bag/ros2bag/verb/burst.py | 11 +- ros2bag/ros2bag/verb/play.py | 51 +- rosbag2_cpp/CMakeLists.txt | 6 +- rosbag2_cpp/include/rosbag2_cpp/logging.hpp | 24 +- .../include/rosbag2_cpp/service_utils.hpp | 19 +- rosbag2_cpp/src/rosbag2_cpp/info.cpp | 17 - rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp | 148 ++-- .../test/rosbag2_cpp/test_service_utils.cpp | 15 + rosbag2_py/rosbag2_py/__init__.py | 2 + rosbag2_py/rosbag2_py/__init__.pyi | 2 +- rosbag2_py/rosbag2_py/_storage.pyi | 9 +- rosbag2_py/rosbag2_py/_transport.pyi | 30 +- rosbag2_py/src/rosbag2_py/_storage.cpp | 20 +- rosbag2_py/src/rosbag2_py/_transport.cpp | 14 +- .../rosbag2_storage/storage_filter.hpp | 39 +- rosbag2_storage_mcap/CMakeLists.txt | 13 +- rosbag2_storage_mcap/src/mcap_storage.cpp | 133 +++- .../test_mcap_storage.cpp | 1 + .../test_mcap_topic_filter.cpp | 424 ++++++++++++ rosbag2_storage_sqlite3/CMakeLists.txt | 8 + rosbag2_storage_sqlite3/package.xml | 1 + .../sqlite_storage.cpp | 133 +++- .../test_sqlite_storage.cpp | 92 --- .../test_sqlite_topic_filter.cpp | 424 ++++++++++++ .../rosbag2_test_common/client_manager.hpp | 16 +- .../rosbag2_test_common/service_manager.hpp | 50 +- rosbag2_transport/CMakeLists.txt | 1 + .../rosbag2_transport/play_options.hpp | 37 +- .../include/rosbag2_transport/player.hpp | 18 + .../player_service_client.hpp | 135 ++++ .../config_options_from_node_params.cpp | 46 +- .../src/rosbag2_transport/logging.hpp | 24 +- .../src/rosbag2_transport/play_options.cpp | 18 +- .../src/rosbag2_transport/player.cpp | 419 +++++++++-- .../player_service_client.cpp | 314 +++++++++ .../test/resources/player_node_params.yaml | 9 +- .../test/rosbag2_transport/mock_player.hpp | 12 + .../mock_sequential_reader.hpp | 18 +- .../rosbag2_play_duration_until_fixture.hpp | 4 +- .../rosbag2_play_test_fixture.hpp | 3 + .../test/rosbag2_transport/test_burst.cpp | 115 +++- .../test_composable_player.cpp | 19 +- .../test/rosbag2_transport/test_play.cpp | 649 +++++++++++++++++- .../test/rosbag2_transport/test_play_loop.cpp | 6 +- .../test/rosbag2_transport/test_play_next.cpp | 16 +- 45 files changed, 3110 insertions(+), 455 deletions(-) create mode 100644 rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_topic_filter.cpp create mode 100644 rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_topic_filter.cpp create mode 100644 rosbag2_transport/include/rosbag2_transport/player_service_client.hpp create mode 100644 rosbag2_transport/src/rosbag2_transport/player_service_client.cpp diff --git a/ros2bag/ros2bag/verb/burst.py b/ros2bag/ros2bag/verb/burst.py index c2c0876879..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 @@ -41,8 +42,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 +95,8 @@ 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 + 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 b2c84f2e80..fcf866f124 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -18,12 +18,14 @@ 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 from ros2cli.node import NODE_NAME_PREFIX from rosbag2_py import Player from rosbag2_py import PlayOptions +from rosbag2_py import ServiceRequestsSource from rosbag2_py import StorageOptions import yaml @@ -49,16 +51,23 @@ def add_arguments(self, parser, cli_name): # noqa: D102 '-r', '--rate', type=check_positive_float, default=1.0, help='rate at which to play back messages. Valid range > 0.0.') parser.add_argument( - '--topics', type=str, default=[], nargs='+', + '--topics', type=str, default=[], metavar='topic', nargs='+', help='Space-delimited list of topics to play.') + parser.add_argument( + '--services', type=str, default=[], metavar='service', nargs='+', + 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 ' - 'specified, all topics will be replayed.') + help='Play only topics and services matches with regular expression.') + parser.add_argument( + '-x', '--exclude-regex', default='', + help='regular expressions to exclude topics and services from replay.') + parser.add_argument( + '--exclude-topics', type=str, default=[], metavar='topic', nargs='+', + help='Space-delimited list of topics not to play.') 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.') + '--exclude-services', type=str, default=[], metavar='service', nargs='+', + help='Space-delimited list of services not to play.') 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.') @@ -144,6 +153,15 @@ def add_arguments(self, parser, cli_name): # noqa: D102 'By default, if loaned message can be used, messages are published as loaned ' 'message. It can help to reduce the number of data copies, so there is a greater ' 'benefit for sending big data.') + parser.add_argument( + '--publish-service-requests', action='store_true', default=False, + help='Publish recorded service requests instead of recorded service events') + parser.add_argument( + '--service-requests-source', default='service_introspection', + choices=['service_introspection', 'client_introspection'], + help='Determine the source of the service requests to be replayed. This option only ' + 'makes sense if the "--publish-service-requests" option is set. By default,' + ' the service requests replaying from recorded service introspection message.') def get_playback_until_from_arg_group(self, playback_until_sec, playback_until_nsec) -> int: nano_scale = 1000 * 1000 * 1000 @@ -182,8 +200,19 @@ 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 + play_options.services_to_filter = convert_service_to_service_event_topic(args.services) + + play_options.regex_to_filter = args.regex + + play_options.exclude_regex_to_filter = args.exclude_regex + + play_options.exclude_topics_to_filter = args.exclude_topics if args.exclude_topics else [] + + play_options.exclude_service_events_to_filter = \ + convert_service_to_service_event_topic(args.exclude_services) + play_options.topic_qos_profile_overrides = qos_profile_overrides play_options.loop = args.loop play_options.topic_remapping_options = topic_remapping @@ -200,6 +229,12 @@ def main(self, *, args): # noqa: D102 play_options.start_offset = args.start_offset play_options.wait_acked_timeout = args.wait_for_all_acked play_options.disable_loan_message = args.disable_loan_message + play_options.publish_service_requests = args.publish_service_requests + if not args.service_requests_source or \ + args.service_requests_source == 'service_introspection': + play_options.service_requests_source = ServiceRequestsSource.SERVICE_INTROSPECTION + else: + play_options.service_requests_source = ServiceRequestsSource.CLIENT_INTROSPECTION player = Player() try: diff --git a/rosbag2_cpp/CMakeLists.txt b/rosbag2_cpp/CMakeLists.txt index d1605bc3c7..ae1b32eada 100644 --- a/rosbag2_cpp/CMakeLists.txt +++ b/rosbag2_cpp/CMakeLists.txt @@ -270,7 +270,11 @@ if(BUILD_TESTING) 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}) + target_link_libraries(test_service_utils + ${PROJECT_NAME} + rosbag2_test_common::rosbag2_test_common + ${test_msgs_TARGETS} + ) endif() endif() diff --git a/rosbag2_cpp/include/rosbag2_cpp/logging.hpp b/rosbag2_cpp/include/rosbag2_cpp/logging.hpp index 85fab581c5..f7a5309b79 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/logging.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/logging.hpp @@ -28,36 +28,36 @@ RCUTILS_LOG_INFO_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_CPP_LOG_INFO_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_INFO_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_CPP_LOG_ERROR(...) \ RCUTILS_LOG_ERROR_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_CPP_LOG_ERROR_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_ERROR_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_CPP_LOG_WARN(...) \ RCUTILS_LOG_WARN_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_CPP_LOG_WARN_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_WARN_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_CPP_LOG_DEBUG(...) \ RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_CPP_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_CPP_LOG_DEBUG_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_CPP_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) // *INDENT-ON* diff --git a/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp index 8bab8cd175..11b9f593a3 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/service_utils.hpp @@ -15,15 +15,18 @@ #ifndef ROSBAG2_CPP__SERVICE_UTILS_HPP_ #define ROSBAG2_CPP__SERVICE_UTILS_HPP_ +#include #include #include "rosbag2_cpp/visibility_control.hpp" +#include "service_msgs/msg/service_event_info.hpp" + namespace rosbag2_cpp { ROSBAG2_CPP_PUBLIC bool -is_service_event_topic(const std::string & topic, const std::string & topic_type); +is_service_event_topic(const std::string & topic_name, const std::string & topic_type); // Call this function after is_service_event_topic() return true ROSBAG2_CPP_PUBLIC @@ -36,12 +39,20 @@ std::string service_event_topic_type_to_service_type(const std::string & topic_type); ROSBAG2_CPP_PUBLIC -size_t -get_serialization_size_for_service_metadata_event(); +std::string +service_name_to_service_event_topic_name(const std::string & service_name); ROSBAG2_CPP_PUBLIC std::string -service_name_to_service_event_topic_name(const std::string & service_name); +client_id_to_string(std::array & client_id); + +struct client_id_hash +{ + static_assert( + std::is_same, + service_msgs::msg::ServiceEventInfo::_client_gid_type>::value); + std::size_t operator()(const std::array & client_id) const; +}; } // 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 995595fbb1..74e62f4c7e 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -60,23 +60,6 @@ rosbag2_storage::BagMetadata Info::read_metadata( namespace { -struct client_id_hash -{ - static_assert( - std::is_same, - service_msgs::msg::ServiceEventInfo::_client_gid_type>::value); - std::size_t operator()(const std::array & client_id) const - { - std::hash hasher; - std::size_t seed = 0; - for (const auto & value : client_id) { - // 0x9e3779b9 is from https://cryptography.fandom.com/wiki/Tiny_Encryption_Algorithm - seed ^= hasher(value) + 0x9e3779b9 + (seed << 6) + (seed >> 2); - } - return seed; - } -}; - using client_id = service_msgs::msg::ServiceEventInfo::_client_gid_type; using sequence_set = std::unordered_set; struct service_req_resp_info diff --git a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp index 786c38d8e8..714e9e1e72 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/service_utils.cpp @@ -17,128 +17,80 @@ #include "rcl/service_introspection.h" #include "rosbag2_cpp/service_utils.hpp" -#include "rosidl_typesupport_cpp/message_type_support.hpp" -#include "rosidl_typesupport_cpp/message_type_support_dispatch.hpp" -#include "rosidl_typesupport_introspection_cpp/identifier.hpp" -#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" - #include "service_msgs/msg/service_event_info.hpp" namespace rosbag2_cpp { -const char * service_event_topic_type_postfix = "_Event"; -const char * service_event_topic_type_middle = "/srv/"; +const char * kServiceEventTopicTypePostfix = "_Event"; +const char * kServiceEventTopicTypeMiddle = "/srv/"; +const size_t kServiceEventTopicPostfixLen = strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX); +const size_t kServiceEventTypePostfixLen = strlen(kServiceEventTopicTypePostfix); -bool is_service_event_topic(const std::string & topic, const std::string & topic_type) +bool is_service_event_topic(const std::string & topic_name, const std::string & topic_type) { - if (topic.length() <= strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) { - return false; - } - - std::string end_topic_name = topic.substr( - topic.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)); - - // Should be "/_service_event" - if (end_topic_name != RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) { - return false; - } - - if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { - return false; - } - - // Should include '/srv/' in type - if (topic_type.find(service_event_topic_type_middle) == std::string::npos) { + if (topic_name.length() <= kServiceEventTopicPostfixLen) { return false; + } else { + // The end of the topic name should be "/_service_event" + if (topic_name.substr(topic_name.length() - kServiceEventTopicPostfixLen) != + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + { + return false; + } } - if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { + if (topic_type.length() <= kServiceEventTypePostfixLen) { return false; + } else { + // Should include '/srv/' in type + if (topic_type.find(kServiceEventTopicTypeMiddle) == std::string::npos) { + return false; + } + + return topic_type.compare( + topic_type.length() - kServiceEventTypePostfixLen, + kServiceEventTypePostfixLen, + kServiceEventTopicTypePostfix) == 0; } - - return topic_type.compare( - topic_type.length() - std::strlen(service_event_topic_type_postfix), - std::strlen(service_event_topic_type_postfix), - service_event_topic_type_postfix) == 0; } std::string service_event_topic_name_to_service_name(const std::string & topic_name) { std::string service_name; - if (topic_name.length() <= strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) { + if (topic_name.length() <= kServiceEventTopicPostfixLen) { return service_name; - } - - if (topic_name.substr( - topic_name.length() - - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) != - RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) - { + } else { + // The end of the topic name should be "/_service_event" + if (topic_name.substr(topic_name.length() - kServiceEventTopicPostfixLen) == + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX) + { + service_name = topic_name.substr(0, topic_name.length() - kServiceEventTopicPostfixLen); + } return service_name; } - - service_name = topic_name.substr( - 0, topic_name.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)); - - return service_name; } std::string service_event_topic_type_to_service_type(const std::string & topic_type) { std::string service_type; - if (topic_type.length() <= std::strlen(service_event_topic_type_postfix)) { + if (topic_type.length() <= kServiceEventTypePostfixLen) { return service_type; } - // Should include '/srv/' in type - if (topic_type.find(service_event_topic_type_middle) == std::string::npos) { + if (topic_type.find(kServiceEventTopicTypeMiddle) == std::string::npos) { return service_type; } - if (topic_type.substr(topic_type.length() - std::strlen(service_event_topic_type_postfix)) != - service_event_topic_type_postfix) + if (topic_type.substr(topic_type.length() - kServiceEventTypePostfixLen) != + kServiceEventTopicTypePostfix) { return service_type; } - - service_type = topic_type.substr( - 0, topic_type.length() - strlen(service_event_topic_type_postfix)); + service_type = topic_type.substr(0, topic_type.length() - kServiceEventTypePostfixLen); return service_type; } -size_t get_serialization_size_for_service_metadata_event() -{ - // Since the size is fixed, it only needs to be calculated once. - static size_t size = 0; - - if (size != 0) { - return size; - } - - const rosidl_message_type_support_t * type_support_info = - rosidl_typesupport_cpp::get_message_type_support_handle(); - - // Get the serialized size of service event info - const rosidl_message_type_support_t * type_support_handle = - rosidl_typesupport_cpp::get_message_typesupport_handle_function( - type_support_info, - rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (type_support_handle == nullptr) { - throw std::runtime_error("Cannot get ServiceEventInfo typesupport handle !"); - } - - auto service_event_info = - static_cast( - type_support_handle->data); - - // endian type (4 size) + service event info size + empty request (4 bytes) - // + emtpy response (4 bytes) - size = 4 + service_event_info->size_of_ + 4 + 4; - - return size; -} - std::string service_name_to_service_event_topic_name(const std::string & service_name) { if (service_name.empty()) { @@ -146,14 +98,34 @@ std::string service_name_to_service_event_topic_name(const std::string & service } // If the end of string is RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX, do nothing - if ((service_name.length() > strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) && - (service_name.substr(service_name.length() - strlen(RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) == + if ((service_name.length() > kServiceEventTopicPostfixLen) && + (service_name.substr(service_name.length() - kServiceEventTopicPostfixLen) == RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX)) { return service_name; } - return service_name + RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX; } +std::string client_id_to_string(std::array & client_id) +{ + // output format: + // xx-xx-xx-xx-xx-xx-xx-xx-xx-xx-xx-xx-xx-xx-xx-xx + std::string client_id_string = std::to_string(client_id[0]); + for (int i = 1; i < 16; i++) { + client_id_string += "-" + std::to_string(client_id[i]); + } + return client_id_string; +} + +std::size_t client_id_hash::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; +} } // namespace rosbag2_cpp diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp index c1dfdc531c..6434a5ed57 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_service_utils.cpp @@ -18,11 +18,16 @@ #include #include "rosbag2_cpp/service_utils.hpp" +#include "rosbag2_test_common/memory_management.hpp" +#include "test_msgs/srv/basic_types.hpp" using namespace ::testing; // NOLINT +using namespace rosbag2_test_common; // NOLINT class ServiceUtilsTest : public Test { +public: + MemoryManagement memory_management_; }; TEST_F(ServiceUtilsTest, check_is_service_event_topic) @@ -91,3 +96,13 @@ TEST_F(ServiceUtilsTest, check_service_name_to_service_event_topic_name) ); } } + +TEST_F(ServiceUtilsTest, check_client_id_to_string) +{ + service_msgs::msg::ServiceEventInfo::_client_gid_type client_id = { + 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16 + }; + std::string expected_string = "1-2-3-4-5-6-7-8-9-10-11-12-13-14-15-16"; + + EXPECT_EQ(rosbag2_cpp::client_id_to_string(client_id), expected_string); +} diff --git a/rosbag2_py/rosbag2_py/__init__.py b/rosbag2_py/rosbag2_py/__init__.py index f848f3404b..8338fd1047 100644 --- a/rosbag2_py/rosbag2_py/__init__.py +++ b/rosbag2_py/rosbag2_py/__init__.py @@ -56,6 +56,7 @@ from rosbag2_py._transport import ( Player, PlayOptions, + ServiceRequestsSource, Recorder, RecordOptions, bag_rewrite, @@ -94,6 +95,7 @@ 'Info', 'Player', 'PlayOptions', + 'ServiceRequestsSource', 'Recorder', 'RecordOptions', ] diff --git a/rosbag2_py/rosbag2_py/__init__.pyi b/rosbag2_py/rosbag2_py/__init__.pyi index d0baa15c7a..5ef2fa4030 100644 --- a/rosbag2_py/rosbag2_py/__init__.pyi +++ b/rosbag2_py/rosbag2_py/__init__.pyi @@ -3,5 +3,5 @@ from rosbag2_py._info import Info as Info from rosbag2_py._reader import SequentialCompressionReader as SequentialCompressionReader, SequentialReader as SequentialReader, get_registered_readers as get_registered_readers from rosbag2_py._reindexer import Reindexer as Reindexer from rosbag2_py._storage import BagMetadata as BagMetadata, ConverterOptions as ConverterOptions, FileInformation as FileInformation, MessageDefinition as MessageDefinition, MetadataIo as MetadataIo, ReadOrder as ReadOrder, ReadOrderSortBy as ReadOrderSortBy, StorageFilter as StorageFilter, StorageOptions as StorageOptions, TopicInformation as TopicInformation, TopicMetadata as TopicMetadata, get_default_storage_id as get_default_storage_id -from rosbag2_py._transport import PlayOptions as PlayOptions, Player as Player, RecordOptions as RecordOptions, Recorder as Recorder, bag_rewrite as bag_rewrite +from rosbag2_py._transport import PlayOptions as PlayOptions, Player as Player, RecordOptions as RecordOptions, Recorder as Recorder, ServiceRequestsSource as ServiceRequestsSource, bag_rewrite as bag_rewrite from rosbag2_py._writer import SequentialCompressionWriter as SequentialCompressionWriter, SequentialWriter as SequentialWriter, get_registered_compressors as get_registered_compressors, get_registered_serializers as get_registered_serializers, get_registered_writers as get_registered_writers diff --git a/rosbag2_py/rosbag2_py/_storage.pyi b/rosbag2_py/rosbag2_py/_storage.pyi index fd1b5cb18b..b07fefef12 100644 --- a/rosbag2_py/rosbag2_py/_storage.pyi +++ b/rosbag2_py/rosbag2_py/_storage.pyi @@ -91,10 +91,13 @@ class ReadOrderSortBy: def value(self) -> int: ... class StorageFilter: + exclude_service_events: List[str] + exclude_topics: List[str] + regex: str + regex_to_exclude: str + services_events: List[str] topics: List[str] - topics_regex: str - topics_regex_to_exclude: str - def __init__(self, topics: List[str] = ..., topics_regex: str = ..., topics_regex_to_exclude: str = ...) -> None: ... + def __init__(self, topics: List[str] = ..., services_events: List[str] = ..., regex: str = ..., exclude_topics: List[str] = ..., exclude_service_events: List[str] = ..., regex_to_exclude: str = ...) -> None: ... class StorageOptions: custom_data: Dict[str,str] diff --git a/rosbag2_py/rosbag2_py/_transport.pyi b/rosbag2_py/rosbag2_py/_transport.pyi index 2d3f771100..aec4911d02 100644 --- a/rosbag2_py/rosbag2_py/_transport.pyi +++ b/rosbag2_py/rosbag2_py/_transport.pyi @@ -1,4 +1,4 @@ -from typing import Any, List +from typing import Any, ClassVar, List import datetime import rosbag2_py._storage @@ -10,18 +10,23 @@ class PlayOptions: delay: float disable_keyboard_controls: bool disable_loan_message: bool + exclude_regex_to_filter: str + exclude_service_events_to_filter: List[str] + exclude_topics_to_filter: List[str] loop: bool node_prefix: str playback_duration: float playback_until_timestamp: int + publish_service_requests: bool rate: float read_ahead_queue_size: int + regex_to_filter: str + service_requests_source: Any + services_to_filter: List[str] start_offset: float start_paused: bool topic_qos_profile_overrides: dict topic_remapping_options: List[str] - topics_regex_to_exclude: str - topics_regex_to_filter: str topics_to_filter: List[str] wait_acked_timeout: int def __init__(self) -> None: ... @@ -64,4 +69,23 @@ class Recorder: def cancel(self, *args, **kwargs) -> Any: ... def record(self, storage_options: rosbag2_py._storage.StorageOptions, record_options: RecordOptions, node_name: str = ...) -> None: ... +class ServiceRequestsSource: + __doc__: ClassVar[str] = ... # read-only + __members__: ClassVar[dict] = ... # read-only + CLIENT_INTROSPECTION: ClassVar[ServiceRequestsSource] = ... + SERVICE_INTROSPECTION: ClassVar[ServiceRequestsSource] = ... + __entries: ClassVar[dict] = ... + def __init__(self, value: int) -> None: ... + def __eq__(self, other: object) -> bool: ... + def __getstate__(self) -> int: ... + def __hash__(self) -> int: ... + def __index__(self) -> int: ... + def __int__(self) -> int: ... + def __ne__(self, other: object) -> bool: ... + def __setstate__(self, state: int) -> None: ... + @property + def name(self) -> str: ... + @property + def value(self) -> int: ... + def bag_rewrite(arg0: List[rosbag2_py._storage.StorageOptions], arg1: str) -> None: ... diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index 6e794e89fb..8f0ea0ccc9 100644 --- a/rosbag2_py/src/rosbag2_py/_storage.cpp +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -127,15 +127,21 @@ PYBIND11_MODULE(_storage, m) { pybind11::class_(m, "StorageFilter") .def( - pybind11::init, std::string, std::string>(), + pybind11::init< + std::vector, std::vector, std::string, + std::vector, std::vector, std::string>(), pybind11::arg("topics") = std::vector(), - pybind11::arg("topics_regex") = "", - pybind11::arg("topics_regex_to_exclude") = "") + pybind11::arg("services_events") = std::vector(), + pybind11::arg("regex") = "", + pybind11::arg("exclude_topics") = std::vector(), + pybind11::arg("exclude_service_events") = std::vector(), + pybind11::arg("regex_to_exclude") = "") .def_readwrite("topics", &rosbag2_storage::StorageFilter::topics) - .def_readwrite("topics_regex", &rosbag2_storage::StorageFilter::topics_regex) - .def_readwrite( - "topics_regex_to_exclude", - &rosbag2_storage::StorageFilter::topics_regex_to_exclude); + .def_readwrite("services_events", &rosbag2_storage::StorageFilter::services_events) + .def_readwrite("regex", &rosbag2_storage::StorageFilter::regex) + .def_readwrite("exclude_topics", &rosbag2_storage::StorageFilter::exclude_topics) + .def_readwrite("exclude_service_events", &rosbag2_storage::StorageFilter::exclude_service_events) + .def_readwrite("regex_to_exclude", &rosbag2_storage::StorageFilter::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 cf571eec33..abd1b6c494 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -456,8 +456,11 @@ 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("topics_regex_to_exclude", &PlayOptions::topics_regex_to_exclude) + .def_readwrite("services_to_filter", &PlayOptions::services_to_filter) + .def_readwrite("regex_to_filter", &PlayOptions::regex_to_filter) + .def_readwrite("exclude_regex_to_filter", &PlayOptions::exclude_regex_to_filter) + .def_readwrite("exclude_topics_to_filter", &PlayOptions::exclude_topics_to_filter) + .def_readwrite("exclude_service_events_to_filter", &PlayOptions::exclude_services_to_filter) .def_property( "topic_qos_profile_overrides", &PlayOptions::getTopicQoSProfileOverrides, @@ -487,6 +490,13 @@ PYBIND11_MODULE(_transport, m) { &PlayOptions::setPlaybackUntilTimestamp) .def_readwrite("wait_acked_timeout", &PlayOptions::wait_acked_timeout) .def_readwrite("disable_loan_message", &PlayOptions::disable_loan_message) + .def_readwrite("publish_service_requests", &PlayOptions::publish_service_requests) + .def_readwrite("service_requests_source", &PlayOptions::service_requests_source) + ; + + py::enum_(m, "ServiceRequestsSource") + .value("SERVICE_INTROSPECTION", rosbag2_transport::ServiceRequestsSource::SERVICE_INTROSPECTION) + .value("CLIENT_INTROSPECTION", rosbag2_transport::ServiceRequestsSource::CLIENT_INTROSPECTION) ; py::class_(m, "RecordOptions") diff --git a/rosbag2_storage/include/rosbag2_storage/storage_filter.hpp b/rosbag2_storage/include/rosbag2_storage/storage_filter.hpp index 4c401505c2..c54de1b069 100644 --- a/rosbag2_storage/include/rosbag2_storage/storage_filter.hpp +++ b/rosbag2_storage/include/rosbag2_storage/storage_filter.hpp @@ -25,18 +25,37 @@ 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. - // If list is empty, the filter is ignored and all messages are played. - std::string topics_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. - std::string topics_regex_to_exclude = ""; + // Service event topic names to whitelist when reading a bag. Only messages + // matching these specified service event topics will be returned. If list + // is empty, the filter is ignored and all messages of service event topics + // are returned. + std::vector services_events; + + // Regular expression of topic names and service name to whitelist when + // playing a bag.Only messages matching these specified topics or services + // will be returned. If the string is empty, the filter is ignored and all + // messages are returned. + std::string regex = ""; + + // Topic names to blacklist when reading a bag. Only messages unmatching these + // topics will be returned. if list is empty, the filter is ignored and all + // messages of topics are returned. + std::vector exclude_topics = {}; + + // Service event topic names to blacklist when reading a bag. Only + // messages unmatching these service event topics will be returned. If list + // is empty, the filter is ignored and all messages of service event topics + // are returned. + std::vector exclude_service_events = {}; + + // Regular expression of topic names and service events names to blacklist when + // playing a bag. Only messages not matching these topics and service events will + // be returned. If the string is empty, the filter is ignored and all messages + // of topics and service events are returned. + std::string regex_to_exclude = ""; }; } // namespace rosbag2_storage diff --git a/rosbag2_storage_mcap/CMakeLists.txt b/rosbag2_storage_mcap/CMakeLists.txt index df10c9a539..88bf75d246 100644 --- a/rosbag2_storage_mcap/CMakeLists.txt +++ b/rosbag2_storage_mcap/CMakeLists.txt @@ -64,10 +64,6 @@ endif() if(${rosbag2_storage_VERSION} VERSION_GREATER_EQUAL 0.15.0) list(APPEND MCAP_COMPILE_DEFS ROSBAG2_STORAGE_MCAP_HAS_YAML_HPP) endif() -# COMPATIBILITY(foxy, galactic, humble, rolling:0.17.x) -if(${rosbag2_storage_VERSION} VERSION_GREATER_EQUAL 0.17.0) - list(APPEND MCAP_COMPILE_DEFS ROSBAG2_STORAGE_MCAP_HAS_STORAGE_FILTER_TOPIC_REGEX) -endif() # COMPATIBILITY(foxy, galactic, humble, rolling:0.17.x, rolling:0.18.x) if(${rosbag2_storage_VERSION} VERSION_GREATER_EQUAL 0.18.0) list(APPEND MCAP_COMPILE_DEFS ROSBAG2_STORAGE_MCAP_HAS_SET_READ_ORDER) @@ -116,6 +112,15 @@ if(BUILD_TESTING) ) target_compile_definitions(test_mcap_storage PRIVATE ${MCAP_COMPILE_DEFS}) + ament_add_gmock(test_mcap_topic_filter test/rosbag2_storage_mcap/test_mcap_topic_filter.cpp) + target_link_libraries(test_mcap_topic_filter + ${PROJECT_NAME} + rosbag2_storage::rosbag2_storage + rosbag2_test_common::rosbag2_test_common + ${std_msgs_TARGETS} + ) + target_compile_definitions(test_mcap_topic_filter PRIVATE ${MCAP_COMPILE_DEFS}) + endif() diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index 1992242586..cbade144c4 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -45,13 +45,11 @@ #include #include #include +#include #include #include #include #include -#ifdef ROSBAG2_STORAGE_MCAP_HAS_STORAGE_FILTER_TOPIC_REGEX - #include -#endif // This is necessary because of using stl types here. It is completely safe, because // a) the member is not accessible from the outside @@ -237,6 +235,35 @@ class ROSBAG2_STORAGE_MCAP_PUBLIC MCAPStorage rosbag2_storage::storage_interfaces::IOFlag io_flag, const std::string & storage_config_uri); + static bool is_topic_name_a_service_event(const std::string_view topic_name); + + /// \brief Check if topic match with the selection criteria by the white list or regex during + /// data read. + /// \details There is assumption that by default all topics shall be selected if none of the + /// filters settled up. If any of the filters are empty those filters shall be ignored. + /// i.e. If white_list or regex not empty they impose restrictions to what topics will be + /// available during read operations. + /// \tparam T - Type of the iterable white_list. e.g. std::vector. + /// \param topic_name - Topic name to be checked. + /// \param white_list - Iterable list of topics that allowed to be available during read. + /// \param regex - String with regular expression for allowed topics to be available during read. + /// \return - true, if the topic passing selection criteria imposed by the white_list or regex, + /// otherwise false. + template + bool is_topic_selected_by_white_list_or_regex(const std::string_view topic_name, + const T & white_list, const std::string & regex); + + /// \brief Check if topic shall be excluded (skipped) during read operations. + /// \tparam T - Type of the iterable black_list. e.g. std::vector. + /// \param topic_name - Topic name to be checked. + /// \param black_list - Iterable list of topics that shall be excluded during read operations. + /// \param regex - String with regular expression for topics that shall be excluded during read + /// operations. + /// \return - true, if the topic name matches with exclusion criteria imposed by the black_list + /// or topic exclude_regex, otherwise false. + template + bool is_topic_in_black_list_or_exclude_regex(const std::string_view topic_name, + const T & black_list, const std::string & regex); void reset_iterator(); bool read_and_enqueue_message(); bool enqueued_message_is_already_read(); @@ -539,6 +566,67 @@ bool MCAPStorage::read_and_enqueue_message() return true; } +bool MCAPStorage::is_topic_name_a_service_event(const std::string_view topic_name) +{ + // The origin definition is RCL_SERVICE_INTROSPECTION_TOPIC_POSTFIX + static const char * service_event_topic_postfix = "/_service_event"; + static const size_t service_event_topic_postfix_len = strlen(service_event_topic_postfix); + size_t topic_name_len = topic_name.length(); + if (topic_name_len <= service_event_topic_postfix_len) { + return false; + } + auto end_topic_name = topic_name.substr(topic_name_len - service_event_topic_postfix_len); + if (end_topic_name != service_event_topic_postfix) { + return false; + } + return true; +} + +template +bool MCAPStorage::is_topic_selected_by_white_list_or_regex(const std::string_view topic_name, + const T & white_list, + const std::string & regex) +{ + // Both white list and regex are empty. i.e. equivalent to include all + if (white_list.empty() && regex.empty()) { + return true; + } + + if (std::find(white_list.begin(), white_list.end(), topic_name) != white_list.end()) { + return true; + } + + if (!regex.empty()) { + std::smatch m; + std::string topic_string(topic_name); + std::regex re(regex); + if (std::regex_match(topic_string, m, re)) { + return true; + } + } + + return false; +} + +template +bool MCAPStorage::is_topic_in_black_list_or_exclude_regex(const std::string_view topic_name, + const T & black_list, + const std::string & regex) +{ + if (std::find(black_list.begin(), black_list.end(), topic_name) != black_list.end()) { + return true; + } + if (!regex.empty()) { + std::smatch m; + std::string topic_string(topic_name); + std::regex re(regex); + if (std::regex_match(topic_string, m, re)) { + return true; + } + } + return false; +} + void MCAPStorage::reset_iterator() { ensure_summary_read(); @@ -554,26 +642,27 @@ void MCAPStorage::reset_iterator() options.endTime = mcap::MaxTime; } options.readOrder = read_order_; - if (!storage_filter_.topics.empty()) { - options.topicFilter = [this](std::string_view topic) { - for (const auto & match_topic : storage_filter_.topics) { - if (match_topic == topic) { - return true; - } + + auto topic_filter = [this](std::string_view topic) { + bool topic_a_service_event = is_topic_name_a_service_event(topic); + + const auto & include_list = + topic_a_service_event ? storage_filter_.services_events : storage_filter_.topics; + + const auto & exclude_list = topic_a_service_event ? storage_filter_.exclude_service_events + : storage_filter_.exclude_topics; + // if topic not found in exclude list or regex_to_exclude + if (!is_topic_in_black_list_or_exclude_regex(topic, exclude_list, + storage_filter_.regex_to_exclude)) { + // if topic selected by include list or regex + if (is_topic_selected_by_white_list_or_regex(topic, include_list, storage_filter_.regex)) { + return true; } - return false; - }; - } -#ifdef ROSBAG2_STORAGE_MCAP_HAS_STORAGE_FILTER_TOPIC_REGEX - if (!storage_filter_.topics_regex.empty()) { - options.topicFilter = [this](std::string_view topic) { - std::smatch m; - std::string topic_string(topic); - std::regex re(storage_filter_.topics_regex); - return std::regex_match(topic_string, m, re); - }; - } -#endif + } + return false; + }; + options.topicFilter = topic_filter; + linear_view_ = std::make_unique(mcap_reader_->readMessages(OnProblem, options)); linear_iterator_ = std::make_unique(linear_view_->begin()); diff --git a/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_storage.cpp b/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_storage.cpp index 27b5bb745d..03d886a6a5 100644 --- a/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_storage.cpp +++ b/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_storage.cpp @@ -320,4 +320,5 @@ TEST_F(TemporaryDirectoryFixture, can_write_mcap_with_zstd_configured_from_yaml) EXPECT_THAT(definitions, ElementsAreArray({definition})); } } + #endif // #ifdef ROSBAG2_STORAGE_MCAP_HAS_STORAGE_OPTIONS diff --git a/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_topic_filter.cpp b/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_topic_filter.cpp new file mode 100644 index 0000000000..0974d7e015 --- /dev/null +++ b/rosbag2_storage_mcap/test/rosbag2_storage_mcap/test_mcap_topic_filter.cpp @@ -0,0 +1,424 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rosbag2_storage/storage_factory.hpp" +#include "rosbag2_storage/storage_options.hpp" +#include "rosbag2_test_common/memory_management.hpp" +#include "std_msgs/msg/string.hpp" + +#include + +#include +#include +#include + +using namespace ::testing; // NOLINT + +namespace fs = std::filesystem; + +class McapTopicFilterTestFixture : public testing::Test +{ +public: + std::shared_ptr + open_test_bag_for_read_only() + { + rosbag2_storage::StorageOptions storage_options{}; + storage_options.storage_id = storage_id_; + storage_options.uri = (fs::path(temporary_dir_path_str_) / "rosbag.mcap").generic_string(); + return storage_factory_.open_read_only(storage_options); + } + +protected: + // Per-test-suite set-up. + // Called before the first test in this test suite. + static void SetUpTestSuite() + { + fs::path tmp_dir_path{fs::temp_directory_path() / "tmp_mcap_test_dir_"}; + temporary_dir_path_str_ = tmp_dir_path.generic_string(); + fs::remove_all(tmp_dir_path); + fs::create_directories(tmp_dir_path); + + std::vector topics = {"topic1", "service_topic1/_service_event", "topic2", + "service_topic2/_service_event", "topic3"}; + + rosbag2_storage::TopicMetadata topic_metadata_1 = { + 1u, topics[0], "std_msgs/msg/String", "cdr", {rclcpp::QoS(1)}, "type_hash1"}; + rosbag2_storage::TopicMetadata topic_metadata_2 = { + 2u, topics[1], "std_msgs/msg/String", "cdr", {rclcpp::QoS(2)}, "type_hash2"}; + rosbag2_storage::TopicMetadata topic_metadata_3 = { + 3u, topics[2], "std_msgs/msg/String", "cdr", {rclcpp::QoS(3)}, "type_hash3"}; + rosbag2_storage::TopicMetadata topic_metadata_4 = { + 4u, topics[3], "std_msgs/msg/String", "cdr", {rclcpp::QoS(4)}, "type_hash4"}; + rosbag2_storage::TopicMetadata topic_metadata_5 = { + 5u, topics[4], "std_msgs/msg/String", "cdr", {rclcpp::QoS(5)}, "type_hash5"}; + + const rosbag2_storage::MessageDefinition definition = {"std_msgs/msg/String", "ros2msg", + "string data", ""}; + + std::vector> + string_messages = { + std::make_tuple("topic1 message", 1, topic_metadata_1, definition), + std::make_tuple("service event topic 1 message", 2, topic_metadata_2, definition), + std::make_tuple("topic2 message", 3, topic_metadata_3, definition), + std::make_tuple("service event topic 2 message", 4, topic_metadata_4, definition), + std::make_tuple("topic3 message", 5, topic_metadata_5, definition)}; + + rosbag2_storage::StorageFactory factory; + rosbag2_storage::StorageOptions options; + options.storage_id = storage_id_; + options.uri = (fs::path(temporary_dir_path_str_) / "rosbag").generic_string(); + + // Write test data + { + auto rw_writer = factory.open_read_write(options); + write_messages_to_mcap(string_messages, rw_writer); + } + } + + static void write_messages_to_mcap( + const std::vector> & messages, + const std::shared_ptr & rw_storage) + { + auto memory_management = std::make_unique(); + for (const auto & [string_message, time_stamp, topic_metadata, message_definition] : messages) { + rw_storage->create_topic(topic_metadata, message_definition); + auto bag_message = std::make_shared(); + auto std_string_msg = std::make_shared(); + std_string_msg->data = string_message; + bag_message->serialized_data = memory_management->serialize_message(std_string_msg); + bag_message->recv_timestamp = time_stamp; + bag_message->send_timestamp = time_stamp; + bag_message->topic_name = topic_metadata.name; + rw_storage->write(bag_message); + } + } + + // Per-test-suite tear-down. + // Called after the last test in this test suite. + static void TearDownTestSuite() + { + if (fs::remove_all(fs::path(temporary_dir_path_str_)) == 0) { + std::cerr << "Failed to remove temporary directory\n"; + } + } + + // The storage factory shall persist while returned storage object persist + rosbag2_storage::StorageFactory storage_factory_; + static const char storage_id_[]; + static std::string temporary_dir_path_str_; +}; + +const char McapTopicFilterTestFixture::storage_id_[] = "mcap"; + +// Note: It is safe to use static string here since we don't expect to use temporary_dir_path_ in +// other static variables or constants initialization or in other translation units. +// Note: The temporary_dir_path_ shall not be used in the McapTopicFilterTestFixture ctor since its +// initialization deferred to the SetUpTestSuite() +std::string McapTopicFilterTestFixture::temporary_dir_path_str_; // NOLINT + +TEST_F(McapTopicFilterTestFixture, CanSelectTopicsAndServicesWithEmptyFilters) +{ + auto readable_storage = open_test_bag_for_read_only(); + + rosbag2_storage::StorageFilter storage_filter{}; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto fifth_message = readable_storage->read_next(); + EXPECT_THAT(fifth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, CanSelectWithTopicsListOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.topics = {"topic1"}; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, CanSelectWithServiceEventsListOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.services_events = {"service_topic2/_service_event"}; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, TestResetFilter) +{ + auto readable_storage = open_test_bag_for_read_only(); + + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.topics = {"topic1"}; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); + + readable_storage->reset_filter(); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, CanSelectFromTopicsListAndRegexWithServices) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Set topic list and regex for service + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.topics = {"topic2", "topic3"}; + storage_filter.regex = "service.*"; // Add service + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, CanSelectFromServicesListAndRegexWithTopics) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Set service list and regex for topic + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.services_events = {"service_topic2/_service_event"}; + storage_filter.regex = "topic(1|3)"; // Add topic1 and topic3 + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, CanSelectFromTopicsListAndServiceList) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Set topic list and service list + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.topics = {"topic2", "topic3"}; + storage_filter.services_events = {"service_topic1/_service_event"}; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesWithRegexOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + // No topic list and service list. Only regex + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.regex = ".*topic2.*"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesWithRegexAndNonexistentTopicsList) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Topics list with non-existent topic and regex with topic and service event + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.topics = {"non-existent_topic"}; + storage_filter.regex = ".*topic2.*"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesWithRegexAndNonexistentServicesList) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Service events list with non-existent service and regex with topic and service event + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.services_events = {"non-existent_service_topic/_service_event"}; + storage_filter.regex = ".*topic2.*"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesWithBlackListsAndExcludeRegexOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + // No topic list, service list and regex. + // Set excluded topic list, excluded service list and excluded regex + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.exclude_topics = {"topic1"}; + storage_filter.exclude_service_events = {"service_topic2/_service_event"}; + storage_filter.regex_to_exclude = "^topic3$"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, FilterTopicsAndServicesExcludeOverlapsWithIncludeLists) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Exclude from include lists + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.topics = {"topic1", "topic2", "topic3"}; + storage_filter.services_events = {"service_topic1/_service_event", + "service_topic2/_service_event"}; + storage_filter.exclude_topics = {"topic1"}; + storage_filter.exclude_service_events = {"service_topic2/_service_event"}; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, FilterWithRegexAndExcludeRegex) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Set regex and excluded regex. + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.regex = ".*topic1.*"; + storage_filter.regex_to_exclude = ".*service.*"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(McapTopicFilterTestFixture, FilterWithExcludeRegexOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Set regex_to_exclude only. + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.regex_to_exclude = ".*topic2.*"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto fifth_message = readable_storage->read_next(); + EXPECT_THAT(fifth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} diff --git a/rosbag2_storage_sqlite3/CMakeLists.txt b/rosbag2_storage_sqlite3/CMakeLists.txt index 7980fc77b0..bc814ba143 100644 --- a/rosbag2_storage_sqlite3/CMakeLists.txt +++ b/rosbag2_storage_sqlite3/CMakeLists.txt @@ -83,6 +83,7 @@ if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) find_package(ament_lint_auto REQUIRED) find_package(rosbag2_test_common REQUIRED) + find_package(std_msgs REQUIRED) ament_lint_auto_find_test_dependencies() set(TEST_LINK_LIBRARIES @@ -105,6 +106,13 @@ if(BUILD_TESTING) if(TARGET test_sqlite_storage) target_link_libraries(test_sqlite_storage ${TEST_LINK_LIBRARIES}) endif() + + ament_add_gmock(test_sqlite_topic_filter + test/rosbag2_storage_sqlite3/test_sqlite_topic_filter.cpp + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + if(TARGET test_sqlite_topic_filter) + target_link_libraries(test_sqlite_topic_filter ${TEST_LINK_LIBRARIES} ${std_msgs_TARGETS}) + endif() endif() ament_package() diff --git a/rosbag2_storage_sqlite3/package.xml b/rosbag2_storage_sqlite3/package.xml index 1ec61817dd..f7cceb548d 100644 --- a/rosbag2_storage_sqlite3/package.xml +++ b/rosbag2_storage_sqlite3/package.xml @@ -24,6 +24,7 @@ ament_lint_common ament_cmake_gmock rosbag2_test_common + std_msgs ament_cmake 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 66869c8815..4ce1e98c44 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -543,42 +543,130 @@ void SqliteStorage::prepare_for_writing() "INSERT INTO messages (timestamp, topic_id, data) VALUES (?, ?, ?);"); } -void SqliteStorage::prepare_for_reading() +namespace { - std::string statement_str = "SELECT data, timestamp, topics.name, messages.id " - "FROM messages JOIN topics ON messages.topic_id = topics.id WHERE "; - std::vector where_conditions; +void prepare_included_topics_filter( + const rosbag2_storage::StorageFilter & storage_filter, + std::vector & where_conditions) +{ + std::string included_topic_names_str; + // Add topic names + if (!storage_filter.topics.empty()) { + for (auto & topic : storage_filter.topics) { + included_topic_names_str += "'" + topic + "'"; + if (&topic != &storage_filter.topics.back()) { + included_topic_names_str += ","; + } + } + } - // add topic filter - if (!storage_filter_.topics.empty()) { - // Construct string for selected topics - std::string topic_list{""}; - for (auto & topic : storage_filter_.topics) { - topic_list += "'" + topic + "'"; - if (&topic != &storage_filter_.topics.back()) { - topic_list += ","; + // Add service event topic names + if (!storage_filter.services_events.empty()) { + if (!included_topic_names_str.empty()) { + included_topic_names_str += ","; + } + for (auto & service : storage_filter.services_events) { + included_topic_names_str += "'" + service + "'"; + if (&service != &storage_filter.services_events.back()) { + included_topic_names_str += ","; } } - where_conditions.push_back("(topics.name IN (" + topic_list + "))"); } - // add topic filter based on regular expression - if (!storage_filter_.topics_regex.empty()) { + + std::string topics_filter_str; + if (!included_topic_names_str.empty()) { + topics_filter_str.append("(topics.name IN (" + included_topic_names_str + "))"); + } + + std::string regex_filter_str; + // Add topics filter based on regular expression + if (!storage_filter.regex.empty()) { // Construct string for selected topics - where_conditions.push_back("(topics.name REGEXP '" + storage_filter_.topics_regex + "')"); + regex_filter_str = "(topics.name REGEXP '" + storage_filter.regex + "')"; + } + + static const char * regex_for_all_service_events_str = "(topics.name REGEXP '.*/_service_event')"; + + if (!topics_filter_str.empty() && !regex_filter_str.empty()) { + // Note: Inclusive filter conditions shall be joined with OR + // Note: Even if services_events list or topics list is empty we shall not include regex for + // all service events or for all topics, because storage_filter.regex is not empty and shall + // dominate in this case. + where_conditions.push_back("(" + topics_filter_str + " OR " + regex_filter_str + ")"); + } else if (!topics_filter_str.empty()) { // Note: regex_filter_str is empty in this case + if (!storage_filter.services_events.empty()) { + if (!storage_filter.topics.empty()) { + where_conditions.push_back(topics_filter_str); + } else { // if topics list empty and service_events not empty we shall include all topics + where_conditions.push_back( + "(" + topics_filter_str + " OR NOT " + regex_for_all_service_events_str + ")"); + } + } else if (!storage_filter.topics.empty()) { + where_conditions.push_back( + "(" + topics_filter_str + " OR " + regex_for_all_service_events_str + ")"); + } else { + // This shall never happen unless someone will make incorrect changes in logic + throw std::logic_error("Either service_events list or topics list shall be not empty!"); + } + } else if (!regex_filter_str.empty()) { + where_conditions.push_back(regex_filter_str); + } +} + +void prepare_excluded_topics_filter( + const rosbag2_storage::StorageFilter & storage_filter, + std::vector & where_conditions) +{ + std::string excluded_topic_names_str; + // Add excluded topic name + if (!storage_filter.exclude_topics.empty()) { + for (auto & topic : storage_filter.exclude_topics) { + excluded_topic_names_str += "'" + topic + "'"; + if (&topic != &storage_filter.exclude_topics.back()) { + excluded_topic_names_str += ","; + } + } } + + // Add service event topic name + if (!storage_filter.exclude_service_events.empty()) { + excluded_topic_names_str += ","; + for (auto & service : storage_filter.exclude_service_events) { + excluded_topic_names_str += "'" + service + "'"; + if (&service != &storage_filter.exclude_service_events.back()) { + excluded_topic_names_str += ","; + } + } + } + + if (!excluded_topic_names_str.empty()) { + where_conditions.push_back("(topics.name NOT IN (" + excluded_topic_names_str + "))"); + } + // exclude topics based on regular expressions - if (!storage_filter_.topics_regex_to_exclude.empty()) { + if (!storage_filter.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_.topics_regex_to_exclude + "'))"); + storage_filter.regex_to_exclude + "'))"); } +} +} // namespace + +void SqliteStorage::prepare_for_reading() +{ + std::string statement_str = "SELECT data, timestamp, topics.name, messages.id " + "FROM messages JOIN topics ON messages.topic_id = topics.id WHERE "; + std::vector where_conditions; + + prepare_included_topics_filter(storage_filter_, where_conditions); + prepare_excluded_topics_filter(storage_filter_, where_conditions); const std::string direction_op = read_order_.reverse ? "<" : ">"; const std::string order_direction = read_order_.reverse ? "DESC" : "ASC"; - // add seek head filter + // Add seek head filter // When doing timestamp ordering, we need a secondary ordering on message_id // Timestamp is not required to be unique, but message_id is, so for messages with the same // timestamp we order by the id to have a consistent and deterministic order. @@ -587,17 +675,14 @@ void SqliteStorage::prepare_for_reading() "AND (messages.id " + direction_op + "= " + std::to_string(seek_row_id_) + ")) " "OR (timestamp " + direction_op + " " + std::to_string(seek_time_) + ")) "); - for ( - std::vector::const_iterator it = where_conditions.begin(); - it != where_conditions.end(); ++it) - { + for (auto it = where_conditions.begin(); it != where_conditions.end(); ++it) { statement_str += *it; if (it != where_conditions.end() - 1) { statement_str += " AND "; } } - // add order by time then id + // Add order by time then id statement_str += "ORDER BY messages.timestamp " + order_direction; statement_str += ", messages.id " + order_direction; statement_str += ";"; 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 d6245c9e1f..371a798d1d 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 @@ -563,98 +563,6 @@ TEST_F(StorageTestFixture, does_not_throw_on_message_too_big) { }); } -TEST_F(StorageTestFixture, read_next_returns_filtered_messages_regex) { - std::vector> - string_messages = - {std::make_tuple("topic1 message", 1, "prefix_topic1", "", ""), - std::make_tuple("topic2 message", 2, "topic2", "", ""), - std::make_tuple("topic3 message", 3, "topic3", "", "")}; - - write_messages_to_sqlite(string_messages); - std::unique_ptr readable_storage = - std::make_unique(); - - auto db_filename = (std::filesystem::path(temporary_dir_path_) / "rosbag.db3").generic_string(); - readable_storage->open({db_filename, kPluginID}); - - rosbag2_storage::StorageFilter storage_filter; - storage_filter.topics_regex = "topic.*"; - readable_storage->set_filter(storage_filter); - - EXPECT_TRUE(readable_storage->has_next()); - auto first_message = readable_storage->read_next(); - EXPECT_THAT(first_message->topic_name, Eq("topic2")); - EXPECT_TRUE(readable_storage->has_next()); - auto second_message = readable_storage->read_next(); - EXPECT_THAT(second_message->topic_name, Eq("topic3")); - EXPECT_FALSE(readable_storage->has_next()); - - // Test reset filter - std::unique_ptr readable_storage2 = - std::make_unique(); - - readable_storage2->open({db_filename, kPluginID}); - readable_storage2->set_filter(storage_filter); - readable_storage2->reset_filter(); - - EXPECT_TRUE(readable_storage2->has_next()); - auto third_message = readable_storage2->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("prefix_topic1")); - EXPECT_TRUE(readable_storage2->has_next()); - auto fourth_message = readable_storage2->read_next(); - EXPECT_THAT(fourth_message->topic_name, Eq("topic2")); - EXPECT_TRUE(readable_storage2->has_next()); - auto fifth_message = readable_storage2->read_next(); - EXPECT_THAT(fifth_message->topic_name, Eq("topic3")); - EXPECT_FALSE(readable_storage2->has_next()); -} - -TEST_F(StorageTestFixture, read_next_returns_filtered_messages_topics_regex_to_exclude) { - std::vector> - string_messages = - {std::make_tuple("topic1 message", 1, "prefix_topic1", "", ""), - std::make_tuple("topic2 message", 2, "topic2", "", ""), - std::make_tuple("topic3 message", 3, "topic3", "", "")}; - - write_messages_to_sqlite(string_messages); - std::unique_ptr readable_storage = - std::make_unique(); - - auto db_filename = (std::filesystem::path(temporary_dir_path_) / "rosbag.db3").generic_string(); - readable_storage->open({db_filename, kPluginID}); - - rosbag2_storage::StorageFilter storage_filter; - storage_filter.topics_regex_to_exclude = "prefix.*"; - readable_storage->set_filter(storage_filter); - - EXPECT_TRUE(readable_storage->has_next()); - auto first_message = readable_storage->read_next(); - EXPECT_THAT(first_message->topic_name, Eq("topic2")); - EXPECT_TRUE(readable_storage->has_next()); - auto second_message = readable_storage->read_next(); - EXPECT_THAT(second_message->topic_name, Eq("topic3")); - EXPECT_FALSE(readable_storage->has_next()); - - // Test reset filter - std::unique_ptr readable_storage2 = - std::make_unique(); - - readable_storage2->open({db_filename, kPluginID}); - readable_storage2->set_filter(storage_filter); - readable_storage2->reset_filter(); - - EXPECT_TRUE(readable_storage2->has_next()); - auto third_message = readable_storage2->read_next(); - EXPECT_THAT(third_message->topic_name, Eq("prefix_topic1")); - EXPECT_TRUE(readable_storage2->has_next()); - auto fourth_message = readable_storage2->read_next(); - EXPECT_THAT(fourth_message->topic_name, Eq("topic2")); - EXPECT_TRUE(readable_storage2->has_next()); - auto fifth_message = readable_storage2->read_next(); - EXPECT_THAT(fifth_message->topic_name, Eq("topic3")); - EXPECT_FALSE(readable_storage2->has_next()); -} - TEST(StoragePresetProfileValidation, parse_storage_preset_profile_works) { EXPECT_EQ( rosbag2_storage_plugins::SqliteStorage::parse_preset_profile(""), diff --git a/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_topic_filter.cpp b/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_topic_filter.cpp new file mode 100644 index 0000000000..5428495226 --- /dev/null +++ b/rosbag2_storage_sqlite3/test/rosbag2_storage_sqlite3/test_sqlite_topic_filter.cpp @@ -0,0 +1,424 @@ +// Copyright 2024 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "rosbag2_storage/storage_factory.hpp" +#include "rosbag2_storage/storage_options.hpp" +#include "rosbag2_test_common/memory_management.hpp" +#include "std_msgs/msg/string.hpp" + +using namespace ::testing; // NOLINT + +namespace fs = std::filesystem; + +class SQLiteTopicFilterTestFixture : public testing::Test +{ +public: + std::shared_ptr + open_test_bag_for_read_only() + { + rosbag2_storage::StorageOptions storage_options{}; + storage_options.storage_id = storage_id_; + storage_options.uri = (fs::path(temporary_dir_path_str_) / "rosbag.db3").generic_string(); + return storage_factory_.open_read_only(storage_options); + } + +protected: + // Per-test-suite set-up. + // Called before the first test in this test suite. + static void SetUpTestSuite() + { + fs::path tmp_dir_path{fs::temp_directory_path() / "tmp_sqlite_test_dir_"}; + temporary_dir_path_str_ = tmp_dir_path.generic_string(); + fs::remove_all(tmp_dir_path); + fs::create_directories(tmp_dir_path); + + std::vector topics = {"topic1", "service_topic1/_service_event", "topic2", + "service_topic2/_service_event", "topic3"}; + + rosbag2_storage::TopicMetadata topic_metadata_1 = { + 1u, topics[0], "std_msgs/msg/String", "cdr", {rclcpp::QoS(1)}, "type_hash1"}; + rosbag2_storage::TopicMetadata topic_metadata_2 = { + 2u, topics[1], "std_msgs/msg/String", "cdr", {rclcpp::QoS(2)}, "type_hash2"}; + rosbag2_storage::TopicMetadata topic_metadata_3 = { + 3u, topics[2], "std_msgs/msg/String", "cdr", {rclcpp::QoS(3)}, "type_hash3"}; + rosbag2_storage::TopicMetadata topic_metadata_4 = { + 4u, topics[3], "std_msgs/msg/String", "cdr", {rclcpp::QoS(4)}, "type_hash4"}; + rosbag2_storage::TopicMetadata topic_metadata_5 = { + 5u, topics[4], "std_msgs/msg/String", "cdr", {rclcpp::QoS(5)}, "type_hash5"}; + + const rosbag2_storage::MessageDefinition definition = {"std_msgs/msg/String", "ros2msg", + "string data", ""}; + + std::vector> + string_messages = { + std::make_tuple("topic1 message", 1, topic_metadata_1, definition), + std::make_tuple("service event topic 1 message", 2, topic_metadata_2, definition), + std::make_tuple("topic2 message", 3, topic_metadata_3, definition), + std::make_tuple("service event topic 2 message", 4, topic_metadata_4, definition), + std::make_tuple("topic3 message", 5, topic_metadata_5, definition)}; + + rosbag2_storage::StorageFactory factory; + rosbag2_storage::StorageOptions options; + options.storage_id = storage_id_; + options.uri = (fs::path(temporary_dir_path_str_) / "rosbag").generic_string(); + + // Write test data + { + auto rw_writer = factory.open_read_write(options); + write_messages_to_sqlite(string_messages, rw_writer); + } + } + +// Per-test-suite tear-down. + // Called after the last test in this test suite. + static void TearDownTestSuite() + { + if (fs::remove_all(fs::path(temporary_dir_path_str_)) == 0) { + std::cerr << "Failed to remove temporary directory\n"; + } + } + + static void write_messages_to_sqlite( + const std::vector> & messages, + const std::shared_ptr & rw_storage) + { + auto memory_management = std::make_unique(); + for (const auto & [string_message, time_stamp, topic_metadata, message_definition] : messages) { + rw_storage->create_topic(topic_metadata, message_definition); + auto bag_message = std::make_shared(); + auto std_string_msg = std::make_shared(); + std_string_msg->data = string_message; + bag_message->serialized_data = memory_management->serialize_message(std_string_msg); + bag_message->recv_timestamp = time_stamp; + bag_message->send_timestamp = time_stamp; + bag_message->topic_name = topic_metadata.name; + rw_storage->write(bag_message); + } + } + + // The storage factory shall persist while returned storage object persist + rosbag2_storage::StorageFactory storage_factory_; + static const char storage_id_[]; + static std::string temporary_dir_path_str_; +}; + +const char SQLiteTopicFilterTestFixture::storage_id_[] = "sqlite3"; + +// Note: It is safe to use static string here since we don't expect to use temporary_dir_path_ in +// other static variables or constants initialization or in other translation units. +// Note: The temporary_dir_path_ shall not be used in the SQLiteTopicFilterTestFixture ctor since +// its initialization deferred to the SetUpTestSuite() +std::string SQLiteTopicFilterTestFixture::temporary_dir_path_str_; // NOLINT + +TEST_F(SQLiteTopicFilterTestFixture, CanSelectTopicsAndServicesWithEmptyFilters) +{ + auto readable_storage = open_test_bag_for_read_only(); + + rosbag2_storage::StorageFilter storage_filter{}; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto fifth_message = readable_storage->read_next(); + EXPECT_THAT(fifth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, CanSelectWithTopicsListOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.topics = {"topic1"}; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, CanSelectWithServiceEventsListOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.services_events = {"service_topic2/_service_event"}; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, TestResetFilter) +{ + auto readable_storage = open_test_bag_for_read_only(); + + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.topics = {"topic1"}; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); + + readable_storage->reset_filter(); + + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, CanSelectFromTopicsListAndRegexWithServices) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Set topic list and regex for service + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.topics = {"topic2", "topic3"}; + storage_filter.regex = "service.*"; // Add service + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto fourth_message = readable_storage->read_next(); + EXPECT_THAT(fourth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, CanSelectFromServicesListAndRegexWithTopics) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Set service list and regex for topic + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.services_events = {"service_topic2/_service_event"}; + storage_filter.regex = "topic(1|3)"; // Add topic1 and topic3 + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, CanSelectFromTopicsListAndServiceList) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Set topic list and service list + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.topics = {"topic2", "topic3"}; + storage_filter.services_events = {"service_topic1/_service_event"}; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesWithRegexOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + // No topic list and service list. Only regex + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.regex = ".*topic2.*"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesWithRegexAndNonexistentTopicsList) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Topics list with non-existent topic and regex with topic and service event + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.topics = {"non-existent_topic"}; + storage_filter.regex = ".*topic2.*"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesWithRegexAndNonexistentServicesList) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Service events list with non-existent service and regex with topic and service event + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.services_events = {"non-existent_service_topic/_service_event"}; + storage_filter.regex = ".*topic2.*"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic2/_service_event")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesWithBlackListsAndExcludeRegexOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + // No topic list, service list and regex. + // Set excluded topic list, excluded service list and excluded regex + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.exclude_topics = {"topic1"}; + storage_filter.exclude_service_events = {"service_topic2/_service_event"}; + storage_filter.regex_to_exclude = "^topic3$"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, FilterTopicsAndServicesExcludeOverlapsWithIncludeLists) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Exclude from include lists + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.topics = {"topic1", "topic2", "topic3"}; + storage_filter.services_events = {"service_topic1/_service_event", + "service_topic2/_service_event"}; + storage_filter.exclude_topics = {"topic1"}; + storage_filter.exclude_service_events = {"service_topic2/_service_event"}; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("topic2")); + EXPECT_TRUE(readable_storage->has_next()); + auto third_message = readable_storage->read_next(); + EXPECT_THAT(third_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, FilterWithRegexAndExcludeRegex) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Set regex and excluded regex. + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.regex = ".*topic1.*"; + storage_filter.regex_to_exclude = ".*service.*"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_FALSE(readable_storage->has_next()); +} + +TEST_F(SQLiteTopicFilterTestFixture, FilterWithExcludeRegexOnly) +{ + auto readable_storage = open_test_bag_for_read_only(); + // Set regex_to_exclude only. + rosbag2_storage::StorageFilter storage_filter{}; + storage_filter.regex_to_exclude = ".*topic2.*"; + readable_storage->set_filter(storage_filter); + + EXPECT_TRUE(readable_storage->has_next()); + auto first_message = readable_storage->read_next(); + EXPECT_THAT(first_message->topic_name, Eq("topic1")); + EXPECT_TRUE(readable_storage->has_next()); + auto second_message = readable_storage->read_next(); + EXPECT_THAT(second_message->topic_name, Eq("service_topic1/_service_event")); + EXPECT_TRUE(readable_storage->has_next()); + auto fifth_message = readable_storage->read_next(); + EXPECT_THAT(fifth_message->topic_name, Eq("topic3")); + EXPECT_FALSE(readable_storage->has_next()); +} diff --git a/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp index c9078fe946..a11502a704 100644 --- a/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp +++ b/rosbag2_test_common/include/rosbag2_test_common/client_manager.hpp @@ -15,15 +15,16 @@ #ifndef ROSBAG2_TEST_COMMON__CLIENT_MANAGER_HPP_ #define ROSBAG2_TEST_COMMON__CLIENT_MANAGER_HPP_ +#include #include #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 @@ -32,14 +33,14 @@ class ClientManager : public rclcpp::Node public: explicit ClientManager( std::string service_name, - size_t client_number = 1, + size_t number_of_clients = 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), + service_name_(std::move(service_name)), + number_of_clients_(number_of_clients), enable_service_event_contents_(service_event_contents), enable_client_event_contents_(client_event_contents) { @@ -73,7 +74,7 @@ class ClientManager : public rclcpp::Node introspection_state = RCL_SERVICE_INTROSPECTION_OFF; } - for (size_t i = 0; i < client_number_; i++) { + for (size_t i = 0; i < number_of_clients_; i++) { auto client = create_client(service_name_); client->configure_introspection( get_clock(), rclcpp::SystemDefaultsQoS(), introspection_state); @@ -114,8 +115,7 @@ class ClientManager : public rclcpp::Node if (rclcpp::executors::spin_node_until_future_complete( exec_, get_node_base_interface(), result, timeout) != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_INFO( - rclcpp::get_logger("service_client_manager"), "Failed to get response !"); + RCLCPP_INFO(this->get_logger(), "Failed to get response !"); return false; } } @@ -129,7 +129,7 @@ class ClientManager : public rclcpp::Node typename rclcpp::Service::SharedPtr service_; std::vector clients_; const std::string service_name_; - size_t client_number_; + size_t number_of_clients_; bool enable_service_event_contents_; bool enable_client_event_contents_; }; diff --git a/rosbag2_test_common/include/rosbag2_test_common/service_manager.hpp b/rosbag2_test_common/include/rosbag2_test_common/service_manager.hpp index ea640eccb6..612ea8463f 100644 --- a/rosbag2_test_common/include/rosbag2_test_common/service_manager.hpp +++ b/rosbag2_test_common/include/rosbag2_test_common/service_manager.hpp @@ -29,15 +29,24 @@ class ServiceManager { public: ServiceManager() - : pub_node_(std::make_shared( + : service_node_(std::make_shared( "service_manager_" + std::to_string(rclcpp::Clock().now().nanoseconds()), - rclcpp::NodeOptions().start_parameter_event_publisher(false).enable_rosout(false))) + rclcpp::NodeOptions() + .start_parameter_event_publisher(false) + .enable_rosout(false) + .start_parameter_services(false))), + check_service_ready_node_(std::make_shared( + "check_service_ready_node_" + std::to_string(rclcpp::Clock().now().nanoseconds()), + rclcpp::NodeOptions() + .start_parameter_event_publisher(false) + .enable_rosout(false) + .start_parameter_services(false))) { } ~ServiceManager() { - exit_ = true; + exec_.cancel(); if (thread_.joinable()) { thread_.join(); } @@ -57,31 +66,40 @@ class ServiceManager requests.emplace_back(request); }; - auto service = pub_node_->create_service( + auto service = service_node_->create_service( service_name, std::forward(callback)); services_.emplace(service_name, service); + + auto client = check_service_ready_node_->create_client(service_name); + clients_.emplace_back(client); } 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)); - } - }; + exec_.add_node(service_node_); + thread_ = std::thread( + [this]() { + exec_.spin(); + }); + } - thread_ = std::thread(spin); + bool all_services_ready() + { + for (auto client : clients_) { + if (!client->wait_for_service(std::chrono::seconds(2))) { + return false; + } + } + return true; } private: - std::shared_ptr pub_node_; + std::shared_ptr service_node_; + std::shared_ptr check_service_ready_node_; std::unordered_map services_; - bool exit_ = false; + std::vector clients_; std::thread thread_; + rclcpp::executors::SingleThreadedExecutor exec_; }; } // namespace rosbag2_test_common diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index 32c37cb248..2d120b3db5 100644 --- a/rosbag2_transport/CMakeLists.txt +++ b/rosbag2_transport/CMakeLists.txt @@ -46,6 +46,7 @@ add_library(${PROJECT_NAME} SHARED src/rosbag2_transport/bag_rewrite.cpp src/rosbag2_transport/player.cpp src/rosbag2_transport/play_options.cpp + src/rosbag2_transport/player_service_client.cpp src/rosbag2_transport/reader_writer_factory.cpp src/rosbag2_transport/recorder.cpp src/rosbag2_transport/record_options.cpp diff --git a/rosbag2_transport/include/rosbag2_transport/play_options.hpp b/rosbag2_transport/include/rosbag2_transport/play_options.hpp index 432bd2d15d..e5029c923a 100644 --- a/rosbag2_transport/include/rosbag2_transport/play_options.hpp +++ b/rosbag2_transport/include/rosbag2_transport/play_options.hpp @@ -28,6 +28,11 @@ namespace rosbag2_transport { +enum class ServiceRequestsSource : int8_t +{ + SERVICE_INTROSPECTION = 0, + CLIENT_INTROSPECTION = 1 +}; struct PlayOptions { @@ -38,18 +43,30 @@ 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 (service event topic names) to whitelist when playing a bag. + // Only messages matching these specified services 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. + // List 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 = ""; + std::vector exclude_topics_to_filter = {}; + + // List of service names (service event topic names) to exclude when playing a bag. + // Only messages not matching these specified services will be played. + std::vector exclude_services_to_filter = {}; + + // Regular expression of topic names and service name to exclude when playing a bag. + // Only messages not matching these specified topics and services will be played. + std::string exclude_regex_to_filter = ""; std::unordered_map topic_qos_profile_overrides = {}; bool loop = false; @@ -100,6 +117,12 @@ struct PlayOptions // Disable to publish as loaned message bool disable_loan_message = false; + + // Publish service requests instead of service events + bool publish_service_requests = false; + + // The source of the service request + ServiceRequestsSource service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/include/rosbag2_transport/player.hpp b/rosbag2_transport/include/rosbag2_transport/player.hpp index 12a84bb12a..d83bb69d23 100644 --- a/rosbag2_transport/include/rosbag2_transport/player.hpp +++ b/rosbag2_transport/include/rosbag2_transport/player.hpp @@ -254,12 +254,30 @@ class Player : public rclcpp::Node ROSBAG2_TRANSPORT_PUBLIC void delete_on_play_message_callback(const callback_handle_t & handle); + /// Wait until sent service requests will receive responses from service servers. + /// \note The player node shall be spun in the executor in a parallel thread to be able to wait + /// for responses. + /// \param service_name - Name of the service or service event from what to wait responses. + /// \note is service_name is empty the function will wait until all service requests sent to all + /// service servers will finish. Timeout in this cases will be used for each service name. + /// \param timeout - Timeout in fraction of seconds to wait for. + /// \return true if service requests successfully finished, otherwise false. + ROSBAG2_TRANSPORT_PUBLIC + bool wait_for_sent_service_requests_to_finish( + const std::string & service_name, + std::chrono::duration timeout = std::chrono::seconds(5)); + protected: /// \brief Getter for publishers corresponding to each topic /// \return Hashtable representing topic to publisher map excluding inner clock_publisher 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_service_clients(); + /// \brief Getter for inner clock_publisher /// \return Shared pointer to the inner clock_publisher ROSBAG2_TRANSPORT_PUBLIC diff --git a/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp new file mode 100644 index 0000000000..ec23f5746d --- /dev/null +++ b/rosbag2_transport/include/rosbag2_transport/player_service_client.hpp @@ -0,0 +1,135 @@ +// 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_TRANSPORT__PLAYER_SERVICE_CLIENT_HPP_ +#define ROSBAG2_TRANSPORT__PLAYER_SERVICE_CLIENT_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "rcl/types.h" +#include "rclcpp/generic_client.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "rosbag2_cpp/service_utils.hpp" + +namespace rosbag2_transport +{ + +class PlayerServiceClientManager; + +class PlayerServiceClient final +{ +public: + using ServiceEventType = service_msgs::msg::ServiceEventInfo::_event_type_type; + using ClientGidType = service_msgs::msg::ServiceEventInfo::_client_gid_type; + + explicit + PlayerServiceClient( + std::shared_ptr generic_client, + std::string service_name, + const std::string & service_event_type, + rclcpp::Logger logger, + std::shared_ptr player_service_client_manager); + + const std::string & get_service_name(); + + /// \brief Deserialize message to the type erased service event + /// \param message - Serialized message + /// \return Shared pointer to the byte array with deserialized service event if success, + /// otherwise nullptr + std::shared_ptr deserialize_service_event(const rcl_serialized_message_t & message); + + std::tuple + get_service_event_type_and_client_gid(const std::shared_ptr type_erased_service_event); + + bool is_service_event_include_request_message( + const std::shared_ptr type_erased_service_event); + + void async_send_request(const std::shared_ptr type_erased_service_event); + + /// Wait until sent service requests will receive responses from service servers. + /// \param timeout - Timeout in fraction of seconds to wait for. + /// \return true if service requests successfully finished, otherwise false. + bool wait_for_sent_requests_to_finish( + std::chrono::duration timeout = std::chrono::seconds(5)); + + std::shared_ptr generic_client() + { + return client_; + } + +private: + std::shared_ptr client_; + std::string service_name_; + const rclcpp::Logger logger_; + std::shared_ptr player_service_client_manager_; + // Note: The service_event_ts_lib_ shall be a member variable to make sure that library loaded + // during the liveliness of the instance of this class, since we have raw pointers to its members. + std::shared_ptr service_event_ts_lib_; + + const rosidl_message_type_support_t * service_event_type_ts_; + const rosidl_typesupport_introspection_cpp::MessageMembers * service_event_members_; + + rcutils_allocator_t allocator_ = rcutils_get_default_allocator(); +}; + +class PlayerServiceClientManager final +{ +public: + explicit PlayerServiceClientManager( + std::chrono::seconds request_future_timeout = std::chrono::minutes(30), + size_t maximum_request_future_queue = 100); + + // Timeout future will be discarded and check queue. + bool request_future_queue_is_full(); + + bool register_request_future( + rclcpp::GenericClient::FutureAndRequestId && future_and_request_id, + std::weak_ptr client); + + /// Wait until sent service requests will receive responses from service servers. + /// \param client - Generic service client from which requests was sent. + /// \note If client is a nullptr it will be taken into account all requests from all clients + /// and timeout will be due per each client. + /// \param timeout - Timeout in fraction of seconds to wait for. + /// \return true if service requests successfully finished, otherwise false. + bool wait_for_sent_requests_to_finish( + std::shared_ptr client, + std::chrono::duration timeout = std::chrono::seconds(5)); + +private: + using time_point = std::chrono::steady_clock::time_point; + using FutureAndRequestIdSharedPtr = std::shared_ptr; + using FutureAndRequestIdAndClient = + std::pair>; + std::map request_futures_list_; + std::mutex request_futures_list_mutex_; + + std::chrono::seconds request_future_timeout_; + size_t maximum_request_future_queue_; + + void remove_complete_request_future(); + + void remove_all_timeout_request_future(); +}; + +} // namespace rosbag2_transport + +#endif // ROSBAG2_TRANSPORT__PLAYER_SERVICE_CLIENT_HPP_ diff --git a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp index 08b1a50a86..0621963a7c 100644 --- a/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp +++ b/rosbag2_transport/src/rosbag2_transport/config_options_from_node_params.cpp @@ -132,11 +132,30 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node) play_options.topics_to_filter = node.declare_parameter>( "play.topics_to_filter", std::vector()); - play_options.topics_regex_to_filter = - node.declare_parameter("play.topics_regex_to_filter", ""); + // Convert service name to service event topic name + auto service_list = node.declare_parameter>( + "play.services_to_filter", std::vector()); + for (auto & service : service_list) { + service = rosbag2_cpp::service_name_to_service_event_topic_name(service); + } + play_options.services_to_filter = service_list; - play_options.topics_regex_to_exclude = - node.declare_parameter("play.topics_regex_to_exclude", ""); + play_options.regex_to_filter = + node.declare_parameter("play.regex_to_filter", ""); + + play_options.exclude_regex_to_filter = + node.declare_parameter("play.exclude_regex_to_filter", ""); + + play_options.exclude_topics_to_filter = node.declare_parameter>( + "play.exclude_topics_to_filter", std::vector()); + + // Convert service name to service event topic name + auto exclude_service_list = node.declare_parameter>( + "play.exclude_services_to_filter", std::vector()); + for (auto & service : exclude_service_list) { + service = rosbag2_cpp::service_name_to_service_event_topic_name(service); + } + play_options.exclude_services_to_filter = exclude_service_list; std::string qos_profile_overrides_path = node.declare_parameter("play.qos_profile_overrides_path", ""); @@ -199,6 +218,25 @@ PlayOptions get_play_options_from_node_params(rclcpp::Node & node) play_options.disable_loan_message = node.declare_parameter("play.disable_loan_message", false); + // "SERVICE_INTROSPECTION" or "CLIENT_INTROSPECTION" + auto service_requests_source = + node.declare_parameter("play.service_requests_source", "SERVICE_INTROSPECTION"); + if (service_requests_source == "SERVICE_INTROSPECTION") { + play_options.service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; + } else if (service_requests_source == "CLIENT_INTROSPECTION") { + play_options.service_requests_source = ServiceRequestsSource::CLIENT_INTROSPECTION; + } else { + play_options.service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; + RCLCPP_ERROR( + node.get_logger(), + "play.service_requests_source doesn't support %s. It must be one of SERVICE_INTROSPECTION" + " and CLIENT_INTROSPECTION. Changed it to default value SERVICE_INTROSPECTION.", + service_requests_source.c_str()); + } + + play_options.publish_service_requests = + node.declare_parameter("play.publish_service_request", false); + return play_options; } diff --git a/rosbag2_transport/src/rosbag2_transport/logging.hpp b/rosbag2_transport/src/rosbag2_transport/logging.hpp index bc8bfb8d96..356edc3025 100644 --- a/rosbag2_transport/src/rosbag2_transport/logging.hpp +++ b/rosbag2_transport/src/rosbag2_transport/logging.hpp @@ -28,36 +28,36 @@ RCUTILS_LOG_INFO_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_TRANSPORT_LOG_INFO_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_INFO_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_INFO_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_TRANSPORT_LOG_ERROR(...) \ RCUTILS_LOG_ERROR_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_TRANSPORT_LOG_ERROR_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_ERROR_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_ERROR_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_TRANSPORT_LOG_WARN(...) \ RCUTILS_LOG_WARN_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_TRANSPORT_LOG_WARN_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_WARN_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_WARN_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) #define ROSBAG2_TRANSPORT_LOG_DEBUG(...) \ RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, __VA_ARGS__) #define ROSBAG2_TRANSPORT_LOG_DEBUG_STREAM(args) do { \ - std::stringstream __ss; \ - __ss << args; \ - RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ + std::stringstream __ss; \ + __ss << args; \ + RCUTILS_LOG_DEBUG_NAMED(ROSBAG2_TRANSPORT_PACKAGE_NAME, "%s", __ss.str().c_str()); \ } while (0) // *INDENT-ON* diff --git a/rosbag2_transport/src/rosbag2_transport/play_options.cpp b/rosbag2_transport/src/rosbag2_transport/play_options.cpp index 2b69b71b1e..64861f8938 100644 --- a/rosbag2_transport/src/rosbag2_transport/play_options.cpp +++ b/rosbag2_transport/src/rosbag2_transport/play_options.cpp @@ -31,8 +31,11 @@ Node convert::encode( node["node_prefix"] = play_options.node_prefix; node["rate"] = play_options.rate; node["topics_to_filter"] = play_options.topics_to_filter; - node["topics_regex_to_filter"] = play_options.topics_regex_to_filter; - node["topics_regex_to_exclude"] = play_options.topics_regex_to_exclude; + node["services_to_filter"] = play_options.services_to_filter; + node["regex_to_filter"] = play_options.regex_to_filter; + node["exclude_regex_to_filter"] = play_options.exclude_regex_to_filter; + node["exclude_topics"] = play_options.exclude_topics_to_filter; + node["exclude_services"] = play_options.exclude_services_to_filter; node["topic_qos_profile_overrides"] = YAML::convert>::encode( play_options.topic_qos_profile_overrides); @@ -67,9 +70,16 @@ bool convert::decode( optional_assign(node, "rate", play_options.rate); optional_assign>( node, "topics_to_filter", play_options.topics_to_filter); - optional_assign(node, "topics_regex_to_filter", play_options.topics_regex_to_filter); + optional_assign>( + node, "services_to_filter", play_options.services_to_filter); + optional_assign(node, "regex_to_filter", play_options.regex_to_filter); optional_assign( - node, "topics_regex_to_exclude", play_options.topics_regex_to_exclude); + node, "exclude_regex_to_filter", + play_options.exclude_regex_to_filter); + optional_assign>( + node, "exclude_topics", play_options.exclude_topics_to_filter); + optional_assign>( + node, "exclude_services", play_options.exclude_services_to_filter); optional_assign>( node, "topic_qos_profile_overrides", play_options.topic_qos_profile_overrides); diff --git a/rosbag2_transport/src/rosbag2_transport/player.cpp b/rosbag2_transport/src/rosbag2_transport/player.cpp index c5b5376f02..1aeb7f2c64 100644 --- a/rosbag2_transport/src/rosbag2_transport/player.cpp +++ b/rosbag2_transport/src/rosbag2_transport/player.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -33,9 +34,14 @@ #include "rosbag2_cpp/clocks/time_controller_clock.hpp" #include "rosbag2_cpp/reader.hpp" +#include "rosbag2_cpp/service_utils.hpp" + #include "rosbag2_storage/storage_filter.hpp" #include "rosbag2_storage/qos.hpp" #include "rosbag2_transport/config_options_from_node_params.hpp" +#include "rosbag2_transport/player_service_client.hpp" + +#include "logging.hpp" namespace { @@ -165,10 +171,26 @@ class PlayerImpl /// #add_on_play_message_post_callback void delete_on_play_message_callback(const callback_handle_t & handle); + /// Wait until sent service requests will receive responses from service servers. + /// \note The player node shall be spun in the executor in a parallel thread to be able to wait + /// for responses. + /// \param service_name - Name of the service or service event from what to wait responses. + /// \note if service_name is empty the function will wait until all service requests sent to all + /// service servers will finish. Timeout in this cases will be used for each service name. + /// \param timeout - Timeout in fraction of seconds to wait for. + /// \return true if service requests successfully finished, otherwise false. + bool wait_for_sent_service_requests_to_finish( + const std::string & service_name, + std::chrono::duration timeout = std::chrono::seconds(5)); + /// \brief Getter for publishers corresponding to each topic /// \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_service_clients(); + /// \brief Getter for inner clock_publisher /// \return Shared pointer to the inner clock_publisher rclcpp::Publisher::SharedPtr get_clock_publisher(); @@ -211,6 +233,9 @@ class PlayerImpl std::forward_list on_play_msg_pre_callbacks_; std::forward_list on_play_msg_post_callbacks_; + void run_play_msg_pre_callbacks(rosbag2_storage::SerializedBagMessageSharedPtr message); + void run_play_msg_post_callbacks(rosbag2_storage::SerializedBagMessageSharedPtr message); + class PlayerPublisher final { public: @@ -243,11 +268,15 @@ class PlayerImpl std::shared_ptr publisher_; std::function publish_func_; }; + 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 PlayerPublisherSharedPtr = std::shared_ptr; + using PlayerServiceClientSharedPtr = std::shared_ptr; + std::unordered_map publishers_; + std::unordered_map service_clients_; private: rosbag2_storage::SerializedBagMessageSharedPtr peek_next_message_from_queue(); @@ -317,6 +346,8 @@ class PlayerImpl // defaults std::shared_ptr keyboard_handler_; std::vector keyboard_callbacks_; + + std::shared_ptr player_service_client_manager_; }; PlayerImpl::PlayerImpl( @@ -328,8 +359,33 @@ PlayerImpl::PlayerImpl( : owner_(owner), storage_options_(storage_options), play_options_(play_options), - keyboard_handler_(std::move(keyboard_handler)) + keyboard_handler_(std::move(keyboard_handler)), + player_service_client_manager_(std::make_shared()) { + for (auto & topic : play_options_.topics_to_filter) { + topic = rclcpp::expand_topic_or_service_name( + topic, owner->get_name(), + owner->get_namespace(), false); + } + + for (auto & exclude_topic : play_options_.exclude_topics_to_filter) { + exclude_topic = rclcpp::expand_topic_or_service_name( + exclude_topic, owner->get_name(), + owner->get_namespace(), false); + } + + for (auto & service_event_topic : play_options_.services_to_filter) { + service_event_topic = rclcpp::expand_topic_or_service_name( + service_event_topic, owner->get_name(), + owner->get_namespace(), false); + } + + for (auto & exclude_service_event_topic : play_options_.exclude_services_to_filter) { + exclude_service_event_topic = rclcpp::expand_topic_or_service_name( + exclude_service_event_topic, owner->get_name(), + owner->get_namespace(), false); + } + { std::lock_guard lk(reader_mutex_); reader_ = std::move(reader); @@ -471,19 +527,19 @@ bool PlayerImpl::play() if (timeout == std::chrono::milliseconds(0)) { timeout = std::chrono::milliseconds(-1); } - for (const auto & pub : publishers_) { + for (auto & [topic, pub] : publishers_) { try { - if (!pub.second->generic_publisher()->wait_for_all_acked(timeout)) { + if (!pub->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()); + "Timed out while waiting for all published messages to be acknowledged for topic " + "%s", topic.c_str()); } } catch (std::exception & e) { RCLCPP_ERROR( owner_->get_logger(), "Exception occurred while waiting for all published messages to be acknowledged for " - "topic %s : %s", pub.first.c_str(), e.what()); + "topic %s : %s", topic.c_str(), e.what()); } } } @@ -745,16 +801,46 @@ void PlayerImpl::delete_on_play_message_callback(const callback_handle_t & handl }); } +bool PlayerImpl::wait_for_sent_service_requests_to_finish( + const std::string & service_name, + std::chrono::duration timeout) +{ + bool is_requests_complete = true; + if (!service_name.empty()) { + auto service_event_name = rosbag2_cpp::service_name_to_service_event_topic_name(service_name); + auto client_iter = service_clients_.find(service_event_name); + if (client_iter != service_clients_.end()) { + is_requests_complete = client_iter->second->wait_for_sent_requests_to_finish(timeout); + } else { + is_requests_complete = false; + } + } else { + is_requests_complete = + player_service_client_manager_->wait_for_sent_requests_to_finish(nullptr, timeout); + } + return is_requests_complete; +} + 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, pub] : publishers_) { + topic_to_publisher_map[topic] = pub->generic_publisher(); } return topic_to_publisher_map; } +std::unordered_map> PlayerImpl::get_service_clients() +{ + std::unordered_map> topic_to_client_map; + for (const auto & [service_name, client] : service_clients_) { + topic_to_client_map[service_name] = client->generic_client(); + } + return topic_to_client_map; +} + rclcpp::Publisher::SharedPtr PlayerImpl::get_clock_publisher() { return clock_publisher_; @@ -878,12 +964,94 @@ void PlayerImpl::play_messages_from_queue() } } +namespace +{ +bool allow_topic( + bool is_service, + const std::string & topic_name, + const rosbag2_storage::StorageFilter & storage_filter) +{ + auto & include_topics = storage_filter.topics; + auto & exclude_topics = storage_filter.exclude_topics; + auto & include_services = storage_filter.services_events; + auto & exclude_services = storage_filter.exclude_service_events; + auto & regex = storage_filter.regex; + auto & regex_to_exclude = storage_filter.regex_to_exclude; + + if (is_service) { + if (!exclude_services.empty()) { + auto it = std::find(exclude_services.begin(), exclude_services.end(), topic_name); + if (it != exclude_services.end()) { + return false; + } + } + } else { + if (!exclude_topics.empty()) { + auto it = std::find(exclude_topics.begin(), exclude_topics.end(), topic_name); + if (it != exclude_topics.end()) { + return false; + } + } + } + + if (!regex_to_exclude.empty()) { + std::smatch m; + std::regex re(regex_to_exclude); + + if (std::regex_match(topic_name, m, re)) { + return false; + } + } + + bool set_include = is_service ? !include_services.empty() : !include_topics.empty(); + bool set_regex = !regex.empty(); + + if (set_include || set_regex) { + if (is_service) { + auto iter = std::find(include_services.begin(), include_services.end(), topic_name); + if (iter == include_services.end()) { + // If include_service is set and regex isn't set, service must be in include_service. + if (!set_regex) { + return false; + } + } else { + return true; + } + } else { + auto iter = std::find(include_topics.begin(), include_topics.end(), topic_name); + if (iter == include_topics.end()) { + // If include_service is set and regex isn't set, service must be in include_service. + if (!set_regex) { + return false; + } + } else { + return true; + } + } + + if (set_regex) { + std::smatch m; + std::regex re(regex); + + if (!std::regex_match(topic_name, m, re)) { + return false; + } + } + } + + return true; +} +} // namespace + 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.topics_regex_to_exclude = play_options_.topics_regex_to_exclude; + storage_filter.services_events = play_options_.services_to_filter; + storage_filter.regex = play_options_.regex_to_filter; + storage_filter.regex_to_exclude = play_options_.exclude_regex_to_filter; + storage_filter.exclude_topics = play_options_.exclude_topics_to_filter; + storage_filter.exclude_service_events = play_options_.exclude_services_to_filter; reader_->set_filter(storage_filter); // Create /clock publisher @@ -924,39 +1092,63 @@ 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()) { - 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()) { + const bool is_service_event_topic = rosbag2_cpp::is_service_event_topic(topic.name, topic.type); + if (play_options_.publish_service_requests && is_service_event_topic) { + // Check if sender was created + if (service_clients_.find(topic.name) != service_clients_.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 + ", "; + // filter service event topic to add client if necessary + if (!allow_topic(true, topic.name, storage_filter)) { + continue; + } + + 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 generic_client = owner_->create_generic_client(service_name, service_type); + auto player_client = std::make_shared( + std::move(generic_client), service_name, topic.type, owner_->get_logger(), + player_service_client_manager_); + service_clients_.insert(std::make_pair(topic.name, player_client)); + } catch (const std::runtime_error & e) { + RCLCPP_WARN( + owner_->get_logger(), + "Ignoring a service '%s', reason: %s.", service_name.c_str(), e.what()); + } + } else { + // Check if sender was created + if (publishers_.find(topic.name) != publishers_.end()) { + continue; + } + + // filter topics to add publishers if necessary + if (!allow_topic(is_service_event_topic, topic.name, storage_filter)) { + 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 + ", "; + } + } 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()); } } @@ -987,22 +1179,37 @@ void PlayerImpl::prepare_publishers() reader_->add_event_callbacks(callbacks); } -bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message) +void PlayerImpl::run_play_msg_pre_callbacks( + rosbag2_storage::SerializedBagMessageSharedPtr message) { - bool message_published = false; - auto publisher_iter = publishers_.find(message->topic_name); - if (publisher_iter != publishers_.end()) { - { // 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_) { - if (pre_callback_data.callback != nullptr) { // Sanity check - pre_callback_data.callback(message); - } - } + std::lock_guard lk(on_play_msg_callbacks_mutex_); + for (auto & pre_callback_data : on_play_msg_pre_callbacks_) { + if (pre_callback_data.callback != nullptr) { // Sanity check + pre_callback_data.callback(message); + } + } +} + +void PlayerImpl::run_play_msg_post_callbacks( + rosbag2_storage::SerializedBagMessageSharedPtr message) +{ + std::lock_guard lk(on_play_msg_callbacks_mutex_); + for (auto & post_callback_data : on_play_msg_post_callbacks_) { + if (post_callback_data.callback != nullptr) { // Sanity check + post_callback_data.callback(message); } + } +} +bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr message) +{ + auto pub_iter = publishers_.find(message->topic_name); + if (pub_iter != publishers_.end()) { + // Calling on play message pre-callbacks + run_play_msg_pre_callbacks(message); + bool message_published = false; try { - publisher_iter->second->publish(rclcpp::SerializedMessage(*message->serialized_data)); + pub_iter->second->publish(rclcpp::SerializedMessage(*message->serialized_data)); message_published = true; } catch (const std::exception & e) { RCLCPP_ERROR_STREAM( @@ -1011,14 +1218,100 @@ bool PlayerImpl::publish_message(rosbag2_storage::SerializedBagMessageSharedPtr } // Calling on play message post-callbacks - std::lock_guard lk(on_play_msg_callbacks_mutex_); - for (auto & post_callback_data : on_play_msg_post_callbacks_) { - if (post_callback_data.callback != nullptr) { // Sanity check - post_callback_data.callback(message); + run_play_msg_post_callbacks(message); + return message_published; + } + + // Try to publish message as service request + auto client_iter = service_clients_.find(message->topic_name); + if (play_options_.publish_service_requests && client_iter != service_clients_.end()) { + const auto & service_client = client_iter->second; + auto service_event = service_client->deserialize_service_event(*message->serialized_data); + if (!service_event) { + RCLCPP_ERROR_STREAM( + owner_->get_logger(), "Failed to deserialize service event message for '" << + service_client->get_service_name() << "' service!\n"); + return false; + } + + try { + auto [service_event_type, client_gid] = + service_client->get_service_event_type_and_client_gid(service_event); + // Ignore response message + if (service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_SENT || + service_event_type == service_msgs::msg::ServiceEventInfo::RESPONSE_RECEIVED) + { + return false; + } + + if (play_options_.service_requests_source == ServiceRequestsSource::SERVICE_INTROSPECTION && + service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) + { + return false; + } + + if (play_options_.service_requests_source == ServiceRequestsSource::CLIENT_INTROSPECTION && + service_event_type != service_msgs::msg::ServiceEventInfo::REQUEST_SENT) + { + return false; + } + + if (!service_client->generic_client()->service_is_ready()) { + RCLCPP_ERROR( + owner_->get_logger(), "Service request hasn't been sent. The '%s' service isn't ready !", + service_client->get_service_name().c_str()); + return false; } + + if (!service_client->is_service_event_include_request_message(service_event)) { + if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED) { + RCUTILS_LOG_WARN_ONCE_NAMED( + ROSBAG2_TRANSPORT_PACKAGE_NAME, + "Can't send service request. " + "The configuration of introspection for '%s' was metadata only on service side!", + service_client->get_service_name().c_str()); + } else if (service_event_type == service_msgs::msg::ServiceEventInfo::REQUEST_SENT) { + RCUTILS_LOG_WARN_ONCE_NAMED( + ROSBAG2_TRANSPORT_PACKAGE_NAME, + "Can't send service request. " + "The configuration of introspection for '%s' client [ID: %s]` was metadata only!", + service_client->get_service_name().c_str(), + rosbag2_cpp::client_id_to_string(client_gid).c_str()); + } + return false; + } + } 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()); + return false; } + + // Calling on play message pre-callbacks + run_play_msg_pre_callbacks(message); + + bool message_published = false; + try { + service_client->async_send_request(service_event); + 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()); + } + + // Calling on play message post-callbacks + run_play_msg_post_callbacks(message); + return message_published; } - return message_published; + + RCUTILS_LOG_WARN_ONCE_NAMED( + ROSBAG2_TRANSPORT_PACKAGE_NAME, + "Publisher for topic '%s' not found", message->topic_name.c_str()); + + return false; } void PlayerImpl::add_key_callback( @@ -1377,11 +1670,23 @@ void Player::delete_on_play_message_callback(const Player::callback_handle_t & h pimpl_->delete_on_play_message_callback(handle); } +bool Player::wait_for_sent_service_requests_to_finish( + const std::string & service_name, std::chrono::duration timeout) +{ + return pimpl_->wait_for_sent_service_requests_to_finish(service_name, timeout); +} + std::unordered_map> Player::get_publishers() { return pimpl_->get_publishers(); } +std::unordered_map> Player::get_service_clients() +{ + return pimpl_->get_service_clients(); +} + rclcpp::Publisher::SharedPtr Player::get_clock_publisher() { return pimpl_->get_clock_publisher(); diff --git a/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp new file mode 100644 index 0000000000..90f0dc649e --- /dev/null +++ b/rosbag2_transport/src/rosbag2_transport/player_service_client.cpp @@ -0,0 +1,314 @@ +// 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 "rosbag2_transport/player_service_client.hpp" + +#include "rosbag2_cpp/service_utils.hpp" + +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" + +#include "logging.hpp" + +using namespace std::chrono_literals; + +namespace rosbag2_transport +{ + +PlayerServiceClient::PlayerServiceClient( + std::shared_ptr generic_client, + std::string service_name, + const std::string & service_event_type, + rclcpp::Logger logger, + std::shared_ptr player_service_client_manager) +: client_(std::move(generic_client)), + service_name_(std::move(service_name)), + logger_(std::move(logger)), + player_service_client_manager_(std::move(player_service_client_manager)) +{ + service_event_ts_lib_ = + rclcpp::get_typesupport_library(service_event_type, "rosidl_typesupport_cpp"); + + service_event_type_ts_ = rclcpp::get_message_typesupport_handle( + service_event_type, "rosidl_typesupport_cpp", *service_event_ts_lib_); + + auto service_event_ts_introspection = get_message_typesupport_handle( + service_event_type_ts_, rosidl_typesupport_introspection_cpp::typesupport_identifier); + + service_event_members_ = + reinterpret_cast( + service_event_ts_introspection->data); + + // Sanity checks for service_event_members_ + if (service_event_members_ == nullptr) { + throw std::invalid_argument("service_event_members_ for `" + service_name_ + "` is nullptr"); + } + if (service_event_members_->member_count_ != 3) { + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + std::stringstream ss; + ss << "Expected 3 fields in the service introspection message, but got " << + service_event_members_->member_count_; + throw std::invalid_argument(ss.str()); + } + + if (!service_event_members_->members_[1].is_array_) { + std::stringstream ss; + ss << "The service request for '" << service_name_ << "' is not array.\n"; + throw std::invalid_argument(ss.str()); + } + + if (service_event_members_->members_[1].size_function == nullptr) { + std::stringstream ss; + ss << "size_function() for service request '" << service_name_ << "' is not set.\n"; + throw std::invalid_argument(ss.str()); + } + + if (service_event_members_->members_[1].get_function == nullptr) { + std::stringstream ss; + ss << "get_function() for service request '" << service_name_ << "' is not set.\n"; + throw std::invalid_argument(ss.str()); + } + + if (service_event_members_->init_function == nullptr) { + std::stringstream ss; + ss << "service_event_members_->init_function for '" << service_name_ << "' is not set.\n"; + throw std::invalid_argument(ss.str()); + } + + if (service_event_members_->fini_function == nullptr) { + std::stringstream ss; + ss << "service_event_members_->fini_function for '" << service_name_ << "' is not set.\n"; + throw std::invalid_argument(ss.str()); + } +} + +const std::string & PlayerServiceClient::get_service_name() +{ + return service_name_; +} + +std::shared_ptr +PlayerServiceClient::deserialize_service_event(const rcl_serialized_message_t & message) +{ + auto type_erased_service_event = std::shared_ptr( + new uint8_t[service_event_members_->size_of_], + [fini_function = this->service_event_members_->fini_function](uint8_t * msg) { + fini_function(msg); + delete[] msg; + }); + + service_event_members_->init_function( + type_erased_service_event.get(), rosidl_runtime_cpp::MessageInitialization::ZERO); + + rmw_ret_t ret = + rmw_deserialize(&message, service_event_type_ts_, type_erased_service_event.get()); + if (ret != RMW_RET_OK) { // Failed to deserialize service event message + type_erased_service_event.reset(); + } + return type_erased_service_event; +} + +std::tuple +PlayerServiceClient::get_service_event_type_and_client_gid( + const std::shared_ptr type_erased_service_event) +{ + if (type_erased_service_event) { + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + const auto & info_member = service_event_members_->members_[0]; + + auto service_event_info_ptr = reinterpret_cast( + type_erased_service_event.get() + info_member.offset_); + if (service_event_info_ptr == nullptr) { + throw std::runtime_error("Error: The service_event_info_ptr is nullptr"); + } + return {service_event_info_ptr->event_type, service_event_info_ptr->client_gid}; + } else { + throw std::invalid_argument("Error: The type_erased_service_event is nullptr"); + } +} + +bool PlayerServiceClient::is_service_event_include_request_message( + const std::shared_ptr type_erased_service_event) +{ + if (type_erased_service_event) { + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + const auto & request_member = service_event_members_->members_[1]; + void * request_sequence_ptr = type_erased_service_event.get() + request_member.offset_; + if (request_member.size_function(request_sequence_ptr) > 0) { + return true; + } // else { /* No service request */ } + } else { + throw std::invalid_argument("Error: The type_erased_service_event is nullptr"); + } + return false; +} + +void PlayerServiceClient::async_send_request( + const std::shared_ptr type_erased_service_event) +{ + // members_[0]: service_info, members_[1]: request[<=1], members_[2]: response[<=1] + const auto & request_member = service_event_members_->members_[1]; + void * request_sequence_ptr = type_erased_service_event.get() + request_member.offset_; + if (request_member.size_function(request_sequence_ptr) > 0) { + void * request_ptr = request_member.get_function(request_sequence_ptr, 0); + auto future_and_request_id = client_->async_send_request(request_ptr); + player_service_client_manager_->register_request_future( + std::move(future_and_request_id), client_); + } // else { /* No service request in the service event. Do nothing, just skip it. */ } +} + +bool PlayerServiceClient::wait_for_sent_requests_to_finish(std::chrono::duration timeout) +{ + return player_service_client_manager_->wait_for_sent_requests_to_finish(client_, timeout); +} + +PlayerServiceClientManager::PlayerServiceClientManager( + std::chrono::seconds requst_future_timeout, + size_t maximum_request_future_queue) +: request_future_timeout_(std::chrono::seconds(requst_future_timeout)), + maximum_request_future_queue_(maximum_request_future_queue) +{ +} + +bool PlayerServiceClientManager::request_future_queue_is_full() +{ + std::lock_guard lock(request_futures_list_mutex_); + + // To improve performance, it's not necessary to clean up completed requests and timeout requests + // every time. + if (request_futures_list_.size() < maximum_request_future_queue_) { + return false; + } + + remove_complete_request_future(); + remove_all_timeout_request_future(); + + if (request_futures_list_.size() == maximum_request_future_queue_) { + return true; + } + + return false; +} + +bool PlayerServiceClientManager::register_request_future( + rclcpp::GenericClient::FutureAndRequestId && future_and_request_id, + std::weak_ptr client) +{ + auto future_and_request_id_shared_ptr = + std::make_shared(std::move(future_and_request_id)); + + if (!request_future_queue_is_full()) { + std::lock_guard lock(request_futures_list_mutex_); + auto emplace_result = request_futures_list_.emplace( + std::chrono::steady_clock::now(), + std::make_pair(std::move(future_and_request_id_shared_ptr), client)); + return emplace_result.second; + } else { + ROSBAG2_TRANSPORT_LOG_WARN( + "Client request queue is full. " + "Please consider increasing the length of the queue."); + } + return false; +} + +bool PlayerServiceClientManager::wait_for_sent_requests_to_finish( + std::shared_ptr client, + std::chrono::duration timeout) +{ + auto is_all_futures_ready = [&]() { + for (auto & [timestamp, future_request_id_and_client] : request_futures_list_) { + if (client) { + auto current_client = future_request_id_and_client.second.lock(); + if (current_client == nullptr) { + throw std::runtime_error("request's client is not valid\n"); + } + // Wait for futures only from specified client + if (client != current_client) { + continue; + } + } // else { /* if the client is not specified, wait for futures from all clients */ } + + const auto & future_and_request_id = future_request_id_and_client.first; + if (!future_and_request_id->future.valid()) { + std::stringstream ss; + ss << "request's " << future_and_request_id->request_id << " future is not valid!\n"; + throw std::runtime_error(ss.str()); + } + if (future_and_request_id->wait_for(0s) != std::future_status::ready) { + return false; + } + } + return true; + }; + + auto sleep_time = std::chrono::milliseconds(10); + if (timeout < std::chrono::seconds(1)) { + sleep_time = std::chrono::duration_cast(timeout); + } + using clock = std::chrono::system_clock; + auto start = clock::now(); + + std::lock_guard lock(request_futures_list_mutex_); + while (!is_all_futures_ready() && (clock::now() - start) < timeout) { + std::this_thread::sleep_for(sleep_time); + } + + return is_all_futures_ready(); +} + +void PlayerServiceClientManager::remove_complete_request_future() +{ + std::vector remove_keys; + std::lock_guard lock(request_futures_list_mutex_); + for (auto & [timestamp, future_request_id_and_client] : request_futures_list_) { + if (future_request_id_and_client.first->wait_for(0s) == std::future_status::ready) { + auto client = future_request_id_and_client.second.lock(); + if (client) { + client->remove_pending_request(future_request_id_and_client.first->request_id); + } + remove_keys.emplace_back(timestamp); + } + } + for (auto & key : remove_keys) { + request_futures_list_.erase(key); + } +} + +void PlayerServiceClientManager::remove_all_timeout_request_future() +{ + std::lock_guard lock(request_futures_list_mutex_); + auto current_time = std::chrono::steady_clock::now(); + auto first_iter_without_timeout = + request_futures_list_.lower_bound(current_time - request_future_timeout_); + + if (first_iter_without_timeout == request_futures_list_.begin()) { + return; + } + + auto last_iter_with_timeout = --first_iter_without_timeout; + for (auto iter = request_futures_list_.begin(); iter != last_iter_with_timeout; iter++) { + auto client = iter->second.second.lock(); + if (client) { + client->remove_pending_request(iter->second.first->request_id); + } + } + request_futures_list_.erase(request_futures_list_.begin(), last_iter_with_timeout); + ROSBAG2_TRANSPORT_LOG_WARN( + "Client requests are discarded since timeout. " + "Please consider setting a longer timeout."); +} +} // namespace rosbag2_transport diff --git a/rosbag2_transport/test/resources/player_node_params.yaml b/rosbag2_transport/test/resources/player_node_params.yaml index 56b75ee5f0..79e1b47a93 100644 --- a/rosbag2_transport/test/resources/player_node_params.yaml +++ b/rosbag2_transport/test/resources/player_node_params.yaml @@ -5,8 +5,11 @@ player_params_node: node_prefix: "test" rate: 13.0 topics_to_filter: ["/foo", "/bar"] - topics_regex_to_filter: "[xyz]/topic" - topics_regex_to_exclude: "[abc]/topic" + services_to_filter: ["/service1", "/service2"] + regex_to_filter: "[xyz]/topic_service" + exclude_topics_to_filter: ["/exclude_foo", "/exclude_bar"] + exclude_services_to_filter: ["/exclude_service1", "/exclude_service2"] + exclude_regex_to_filter: "[abc]/topic_service" loop: false clock_publish_frequency: 19.0 clock_publish_on_topic_publish: true @@ -34,6 +37,8 @@ player_params_node: sec: 0 nsec: -999999999 disable_loan_message: false + publish_service_requests: false + service_requests_source: "CLIENT_INTROSPECTION" # SERVICE_INTROSPECTION or CLIENT_INTROSPECTION storage: uri: "path/to/some_bag" diff --git a/rosbag2_transport/test/rosbag2_transport/mock_player.hpp b/rosbag2_transport/test/rosbag2_transport/mock_player.hpp index e08ac4c8ab..9220679b0b 100644 --- a/rosbag2_transport/test/rosbag2_transport/mock_player.hpp +++ b/rosbag2_transport/test/rosbag2_transport/mock_player.hpp @@ -56,6 +56,18 @@ class MockPlayer : public rosbag2_transport::Player return pub_list; } + std::vector get_list_of_clients() + { + std::vector cli_list; + for (const auto & client : this->get_service_clients()) { + cli_list.push_back( + static_cast( + client.second.get())); + } + + return cli_list; + } + size_t get_number_of_registered_pre_callbacks() { return get_number_of_registered_on_play_msg_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 a064704574..ebd314315f 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_events.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_events.empty()) { + for (const auto & filter_service : filter_.services_events) { + if (!messages_[num_read_]->topic_name.compare(filter_service)) { + return true; + } } } num_read_++; diff --git a/rosbag2_transport/test/rosbag2_transport/rosbag2_play_duration_until_fixture.hpp b/rosbag2_transport/test/rosbag2_transport/rosbag2_play_duration_until_fixture.hpp index 84c443f775..b1ecabe092 100644 --- a/rosbag2_transport/test/rosbag2_transport/rosbag2_play_duration_until_fixture.hpp +++ b/rosbag2_transport/test/rosbag2_transport/rosbag2_play_duration_until_fixture.hpp @@ -50,8 +50,8 @@ class RosBag2PlayDurationAndUntilTestFixture : public RosBag2PlayTestFixture static constexpr bool kBool2Value{true}; static constexpr bool kBool3Value{false}; - static constexpr const char * kTopic1Name_{"topic1"}; - static constexpr const char * kTopic2Name_{"topic2"}; + static constexpr const char * kTopic1Name_{"/topic1"}; + static constexpr const char * kTopic2Name_{"/topic2"}; static constexpr const char * kTopic1_{"/topic1"}; static constexpr const char * kTopic2_{"/topic2"}; 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 834afe050e..0d28d86cc5 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_burst.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_burst.cpp @@ -25,10 +25,43 @@ #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_RECEIVED; + 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_RECEIVED; + 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]; @@ -313,18 +346,18 @@ TEST_F(RosBag2PlayTestFixture, burst_bursting_only_filtered_topics) { complex_message1->bool_values = {{true, false, true}}; auto topic_types = std::vector{ - {1u, "topic1", "test_msgs/BasicTypes", "", {}, ""}, - {2u, "topic2", "test_msgs/Arrays", "", {}, ""}, + {1u, "/topic1", "test_msgs/BasicTypes", "", {}, ""}, + {2u, "/topic2", "test_msgs/Arrays", "", {}, ""}, }; std::vector> messages = { - serialize_test_message("topic1", 500, primitive_message1), - serialize_test_message("topic1", 700, primitive_message1), - serialize_test_message("topic1", 900, primitive_message1), - serialize_test_message("topic2", 550, complex_message1), - serialize_test_message("topic2", 750, complex_message1), - serialize_test_message("topic2", 950, complex_message1) + serialize_test_message("/topic1", 500, primitive_message1), + serialize_test_message("/topic1", 700, primitive_message1), + serialize_test_message("/topic1", 900, primitive_message1), + serialize_test_message("/topic2", 550, complex_message1), + serialize_test_message("/topic2", 750, complex_message1), + serialize_test_message("/topic2", 950, complex_message1) }; // Filter allows /topic2, blocks /topic1 @@ -366,3 +399,69 @@ 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) { + if (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") != + std::string::npos) + { + GTEST_SKIP() << "Skipping. The test is know to be flaky on the rmw_connextdds."; + } + const std::string service_name1 = "/test_service1"; + const std::string service_name2 = "/test_service2"; + const std::string service_event_name1 = service_name1 + "/_service_event"; + const std::string service_event_name2 = service_name2 + "/_service_event"; + + auto services_types = std::vector{ + {1u, service_event_name1, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {2u, service_event_name2, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + }; + std::vector> messages = + { + serialize_test_message(service_event_name1, 500, get_service_event_message_basic_types()[0]), + serialize_test_message(service_event_name2, 600, get_service_event_message_basic_types()[0]), + serialize_test_message(service_event_name1, 400, get_service_event_message_basic_types()[1]), + serialize_test_message(service_event_name2, 500, get_service_event_message_basic_types()[1]) + }; + + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); + + srv_->run_services(); + + // Filter allows test_service2, blocks test_service1 + play_options_.services_to_filter = {service_event_name2}; + play_options_.publish_service_requests = true; + 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()); + + // Check services are ready + ASSERT_TRUE(srv_->all_services_ready()); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread([&exec]() {exec.spin();}); + player->play(); + + 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->wait_for_playback_to_finish(); + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish("", 2s)); + + exec.cancel(); + spin_thread.join(); + + EXPECT_EQ(service1_receive_requests.size(), 0); + EXPECT_EQ(service2_receive_requests.size(), 2); +} diff --git a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp index 70826d4189..0c0c3447b6 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_composable_player.cpp @@ -143,8 +143,19 @@ TEST_P(ComposablePlayerTests, player_can_parse_parameters_from_file) { EXPECT_EQ(play_options.rate, 13.0); std::vector topics_to_filter {"/foo", "/bar"}; EXPECT_EQ(play_options.topics_to_filter, topics_to_filter); - EXPECT_EQ(play_options.topics_regex_to_filter, "[xyz]/topic"); - EXPECT_EQ(play_options.topics_regex_to_exclude, "[abc]/topic"); + std::vector services_to_filter { + "/service1/_service_event", + "/service2/_service_event"}; + EXPECT_EQ(play_options.services_to_filter, services_to_filter); + EXPECT_EQ(play_options.regex_to_filter, "[xyz]/topic_service"); + std::vector exclude_topics_to_filter {"/exclude_foo", "/exclude_bar"}; + EXPECT_EQ(play_options.exclude_topics_to_filter, exclude_topics_to_filter); + std::vector exclude_services_to_filter { + "/exclude_service1/_service_event", + "/exclude_service2/_service_event"}; + EXPECT_EQ(play_options.exclude_services_to_filter, exclude_services_to_filter); + EXPECT_EQ(play_options.exclude_regex_to_filter, "[abc]/topic_service"); + std::unordered_map topic_qos_profile_overrides{ std::pair{ "/overrided_topic_qos", @@ -161,6 +172,10 @@ TEST_P(ComposablePlayerTests, player_can_parse_parameters_from_file) { EXPECT_EQ(play_options.start_offset, 999999999); EXPECT_EQ(play_options.wait_acked_timeout, -999999999); EXPECT_EQ(play_options.disable_loan_message, false); + EXPECT_EQ(play_options.publish_service_requests, false); + EXPECT_EQ( + play_options.service_requests_source, + rosbag2_transport::ServiceRequestsSource::CLIENT_INTROSPECTION); EXPECT_EQ(storage_options.uri, uri_str); EXPECT_EQ(storage_options.storage_id, GetParam()); diff --git a/rosbag2_transport/test/rosbag2_transport/test_play.cpp b/rosbag2_transport/test/rosbag2_transport/test_play.cpp index 2cc2c20d17..8b7b40f509 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_storage/qos.hpp" @@ -43,6 +45,70 @@ using namespace rosbag2_transport; // NOLINT using namespace std::chrono_literals; // NOLINT using namespace rosbag2_test_common; // NOLINT +namespace +{ +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_RECEIVED; + test_msgs::srv::BasicTypes_Request request; + request.int32_value = 123; + request.int64_value = 456; + request.string_value = "event_type=REQUEST_RECEIVED"; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + { + auto msg = std::make_shared(); + msg->info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_RECEIVED; + test_msgs::srv::BasicTypes_Request request; + request.int32_value = 456; + request.int64_value = 789; + request.string_value = "event_type=REQUEST_RECEIVED"; + 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 = 789; + request.int64_value = 123; + request.string_value = "event_type=REQUEST_SENT"; + msg->request.emplace_back(request); + messages.push_back(msg); + } + + return messages; +} + +void spin_thread_and_wait_for_sent_service_requests_to_finish( + std::shared_ptr player, + const std::vector && service_name_list) +{ + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread( + [&exec]() { + exec.spin(); + }); + player->play(); + player->wait_for_playback_to_finish(); + + for (const auto & service_name : service_name_list) { + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name, 2s)); + } + exec.cancel(); + if (spin_thread.joinable()) {spin_thread.join();} +} +} // namespace + TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_all_topics) { auto primitive_message1 = get_messages_basic_types()[0]; @@ -108,6 +174,115 @@ 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) +{ + const std::string service_name1 = "/test_service1"; + const std::string service_event_name1 = service_name1 + "/_service_event"; + const std::string service_name2 = "/test_service2"; + const std::string service_event_name2 = service_name2 + "/_service_event"; + + auto services_types = std::vector{ + {1u, service_event_name1, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {2u, service_event_name2, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + }; + std::vector> messages = + { + serialize_test_message(service_event_name1, 500, get_service_event_message_basic_types()[0]), + serialize_test_message(service_event_name2, 600, get_service_event_message_basic_types()[0]), + serialize_test_message(service_event_name1, 400, get_service_event_message_basic_types()[1]), + serialize_test_message(service_event_name2, 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(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); + + srv_->run_services(); + + ASSERT_TRUE(srv_->all_services_ready()); + + play_options_.publish_service_requests = true; + auto player = + std::make_shared(std::move(reader), storage_options_, play_options_); + + spin_thread_and_wait_for_sent_service_requests_to_finish(player, {service_name1, service_name2}); + + 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; + + const std::string topic_name = "/topic1"; + const std::string service_name = "/test_service1"; + const std::string service_event_name = service_name + "/_service_event"; + + auto services_types = std::vector{ + {1u, topic_name, "test_msgs/BasicTypes", "", {}, ""}, + {2u, service_event_name, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + }; + std::vector> messages = + { + serialize_test_message(service_event_name, 500, get_service_event_message_basic_types()[0]), + serialize_test_message(topic_name, 600, topic_msg), + serialize_test_message(service_event_name, 550, get_service_event_message_basic_types()[1]), + serialize_test_message(topic_name, 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(service_name, service_receive_requests); + srv_->run_services(); + ASSERT_TRUE(srv_->all_services_ready()); + + sub_->add_subscription(topic_name, 2); + auto await_received_messages = sub_->spin_subscriptions(); + + play_options_.publish_service_requests = true; + auto player = std::make_shared( + std::move(reader), storage_options_, play_options_); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread([&exec]() {exec.spin();}); + + player->play(); + player->wait_for_playback_to_finish(); + + await_received_messages.get(); + + auto replayed_topic_msg = sub_->get_received_messages(topic_name); + EXPECT_THAT(replayed_topic_msg, SizeIs(Ge(2u))); + EXPECT_THAT( + replayed_topic_msg, + Each(Pointee(Field(&test_msgs::msg::BasicTypes::int64_value, 1111)))); + + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name, 2s)); + exec.cancel(); + spin_thread.join(); + ASSERT_EQ(service_receive_requests.size(), 2); + for (size_t i = 0; i < service_receive_requests.size(); 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]; @@ -188,17 +363,17 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics) complex_message1->bool_values = {{true, false, true}}; auto topic_types = std::vector{ - {1u, "topic1", "test_msgs/BasicTypes", "", {}, ""}, - {2u, "topic2", "test_msgs/Arrays", "", {}, ""}, + {1u, "/topic1", "test_msgs/BasicTypes", "", {}, ""}, + {2u, "/topic2", "test_msgs/Arrays", "", {}, ""}, }; std::vector> messages = - {serialize_test_message("topic1", 500, primitive_message1), - serialize_test_message("topic1", 700, primitive_message1), - serialize_test_message("topic1", 900, primitive_message1), - serialize_test_message("topic2", 550, complex_message1), - serialize_test_message("topic2", 750, complex_message1), - serialize_test_message("topic2", 950, complex_message1)}; + {serialize_test_message("/topic1", 500, primitive_message1), + serialize_test_message("/topic1", 700, primitive_message1), + serialize_test_message("/topic1", 900, primitive_message1), + serialize_test_message("/topic2", 550, complex_message1), + serialize_test_message("/topic2", 750, complex_message1), + serialize_test_message("/topic2", 950, complex_message1)}; // Filter allows /topic2, blocks /topic1 { @@ -297,6 +472,295 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics) } } +TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_services) +{ + const std::string service_name1 = "/test_service1"; + const std::string service_event_name1 = service_name1 + "/_service_event"; + const std::string service_name2 = "/test_service2"; + const std::string service_event_name2 = service_name2 + "/_service_event"; + + auto services_types = std::vector{ + {1u, service_event_name1, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {2u, service_event_name2, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + }; + std::vector> messages = + { + serialize_test_message(service_event_name1, 500, get_service_event_message_basic_types()[0]), + serialize_test_message(service_event_name2, 600, get_service_event_message_basic_types()[0]), + serialize_test_message(service_event_name1, 400, get_service_event_message_basic_types()[1]), + serialize_test_message(service_event_name2, 500, get_service_event_message_basic_types()[1]) + }; + + play_options_.publish_service_requests = true; + + // Filter allows /test_service2, blocks /test_service1 + { + play_options_.services_to_filter = {service_event_name2}; + + srv_.reset(); + srv_ = std::make_shared(); + + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); + + srv_->run_services(); + ASSERT_TRUE(srv_->all_services_ready()); + + 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_); + + // Only need to wait for sent service 2 request to finish + spin_thread_and_wait_for_sent_service_requests_to_finish(player, {service_name2}); + + 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 = {service_event_name1}; + + srv_.reset(); + srv_ = std::make_shared(); + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); + + srv_->run_services(); + ASSERT_TRUE(srv_->all_services_ready()); + + 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_); + // Only need to wait for sent service 1 request to finish + spin_thread_and_wait_for_sent_service_requests_to_finish(player, {service_name1}); + + 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(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); + + srv_->run_services(); + ASSERT_TRUE(srv_->all_services_ready()); + + 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_); + spin_thread_and_wait_for_sent_service_requests_to_finish( + player, {service_name1, service_name2}); + + 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) +{ + const std::string topic_name1 = "/topic1"; + const std::string topic_name2 = "/topic2"; + const std::string service_name1 = "/test_service1"; + const std::string service_event_name1 = service_name1 + "/_service_event"; + const std::string service_name2 = "/test_service2"; + const std::string service_event_name2 = service_name2 + "/_service_event"; + + auto all_types = std::vector{ + {1u, topic_name1, "test_msgs/BasicTypes", "", {}, ""}, + {2u, topic_name2, "test_msgs/BasicTypes", "", {}, ""}, + {3u, service_event_name1, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {4u, service_event_name2, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + }; + + std::vector> messages = + { + serialize_test_message(topic_name1, 500, get_messages_basic_types()[0]), + serialize_test_message(service_event_name1, 520, get_service_event_message_basic_types()[0]), + serialize_test_message(topic_name2, 520, get_messages_basic_types()[0]), + serialize_test_message(service_event_name2, 550, get_service_event_message_basic_types()[0]), + }; + + play_options_.publish_service_requests = true; + // Filter allows all topics, blocks service test_service2 + { + play_options_.topics_to_filter = {topic_name1, topic_name2}; + play_options_.services_to_filter = {service_event_name1}; + + sub_.reset(); + sub_ = std::make_shared(); + sub_->add_subscription(topic_name1, 1); + sub_->add_subscription(topic_name2, 1); + + srv_.reset(); + srv_ = std::make_shared(); + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); + srv_->run_services(); + ASSERT_TRUE(srv_->all_services_ready()); + + 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_); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread([&exec]() {exec.spin();}); + + player->play(); + await_received_messages.get(); + player->wait_for_playback_to_finish(); + + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name1, 2s)); + exec.cancel(); + spin_thread.join(); + + // Filter allow all topics + auto replayed_topic1 = sub_->get_received_messages(topic_name1); + EXPECT_THAT(replayed_topic1, SizeIs(1u)); + auto replayed_topic2 = sub_->get_received_messages(topic_name2); + 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 = {topic_name1}; + play_options_.services_to_filter = { + service_event_name1, service_event_name2}; + + sub_.reset(); + sub_ = std::make_shared(); + sub_->add_subscription(topic_name1, 1); + sub_->add_subscription(topic_name2, 0); + + srv_.reset(); + srv_ = std::make_shared(); + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); + srv_->run_services(); + ASSERT_TRUE(srv_->all_services_ready()); + + 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_); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread([&exec]() {exec.spin();}); + + player->play(); + await_received_messages.get(); + player->wait_for_playback_to_finish(); + + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name1, 2s)); + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name2, 2s)); + exec.cancel(); + spin_thread.join(); + + // Filter allow topic2, block topic1 + auto replayed_topic1 = sub_->get_received_messages(topic_name1); + EXPECT_THAT(replayed_topic1, SizeIs(1u)); + auto replayed_topic2 = sub_->get_received_messages(topic_name2); + 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 = {topic_name1, topic_name2}; + play_options_.services_to_filter = { + service_event_name1, service_event_name2}; + + sub_.reset(); + sub_ = std::make_shared(); + sub_->add_subscription(topic_name1, 1); + sub_->add_subscription(topic_name2, 1); + + srv_.reset(); + srv_ = std::make_shared(); + std::vector> service1_receive_requests; + std::vector> service2_receive_requests; + srv_->setup_service(service_name1, service1_receive_requests); + srv_->setup_service(service_name2, service2_receive_requests); + srv_->run_services(); + ASSERT_TRUE(srv_->all_services_ready()); + + 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_); + + rclcpp::executors::SingleThreadedExecutor exec; + exec.add_node(player); + auto spin_thread = std::thread([&exec]() {exec.spin();}); + + player->play(); + await_received_messages.get(); + player->wait_for_playback_to_finish(); + + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name1, 2s)); + EXPECT_TRUE(player->wait_for_sent_service_requests_to_finish(service_name2, 2s)); + exec.cancel(); + spin_thread.join(); + + // 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]; @@ -310,19 +774,19 @@ TEST_F(RosBag2PlayTestFixture, recorded_messages_are_played_for_filtered_topics_ unknown_message1->int32_value = 42; auto topic_types = std::vector{ - {1u, "topic1", "test_msgs/BasicTypes", "", {}, ""}, - {2u, "topic2", "test_msgs/Arrays", "", {}, ""}, - {3u, "topic3", "unknown_msgs/UnknownType", "", {}, ""}, + {1u, "/topic1", "test_msgs/BasicTypes", "", {}, ""}, + {2u, "/topic2", "test_msgs/Arrays", "", {}, ""}, + {3u, "/topic3", "unknown_msgs/UnknownType", "", {}, ""}, }; std::vector> messages = - {serialize_test_message("topic1", 500, primitive_message1), - serialize_test_message("topic1", 700, primitive_message1), - serialize_test_message("topic1", 900, primitive_message1), - serialize_test_message("topic2", 550, complex_message1), - serialize_test_message("topic2", 750, complex_message1), - serialize_test_message("topic2", 950, complex_message1), - serialize_test_message("topic3", 900, unknown_message1)}; + {serialize_test_message("/topic1", 500, primitive_message1), + serialize_test_message("/topic1", 700, primitive_message1), + serialize_test_message("/topic1", 900, primitive_message1), + serialize_test_message("/topic2", 550, complex_message1), + serialize_test_message("/topic2", 750, complex_message1), + serialize_test_message("/topic2", 950, complex_message1), + serialize_test_message("/topic3", 900, unknown_message1)}; { play_options_.topics_to_filter = {"topic2"}; @@ -439,6 +903,155 @@ TEST_F(RosBag2PlayTestFixture, player_gracefully_exit_by_rclcpp_shutdown_in_paus player->wait_for_playback_to_finish(); } +TEST_F(RosBag2PlayTestFixture, play_service_requests_from_service_introspection_messages) +{ + const std::string service_name = "/test_service1"; + const std::string service_event_name = service_name + "/_service_event"; + auto services_types = std::vector{ + {1u, service_event_name, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + }; + std::vector> messages = + { + serialize_test_message(service_event_name, 5, get_service_event_message_basic_types()[2]), + serialize_test_message(service_event_name, 10, get_service_event_message_basic_types()[0]), + serialize_test_message(service_event_name, 20, get_service_event_message_basic_types()[2]), + serialize_test_message(service_event_name, 25, get_service_event_message_basic_types()[0]), + serialize_test_message(service_event_name, 30, get_service_event_message_basic_types()[2]), + }; + + 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> received_service_requests; + + srv_->setup_service(service_name, received_service_requests); + srv_->run_services(); + ASSERT_TRUE(srv_->all_services_ready()); + + play_options_.publish_service_requests = true; + play_options_.service_requests_source = ServiceRequestsSource::SERVICE_INTROSPECTION; + + auto player = + std::make_shared(std::move(reader), storage_options_, play_options_); + + spin_thread_and_wait_for_sent_service_requests_to_finish(player, {service_name}); + + EXPECT_EQ(received_service_requests.size(), 2); + // expected_request is ServiceEventInfo::REQUEST_RECEIVED + const auto expected_request = get_service_event_message_basic_types()[0]->request[0]; + for (const auto & service_request : received_service_requests) { + EXPECT_EQ(service_request->int32_value, expected_request.int32_value) << + service_request->string_value; + EXPECT_EQ(service_request->int64_value, expected_request.int64_value) << + service_request->string_value; + } +} + +TEST_F(RosBag2PlayTestFixture, play_service_requests_from_client_introspection_messages) +{ + const std::string service_name = "/test_service1"; + const std::string service_event_name = service_name + "/_service_event"; + auto services_types = std::vector{ + {1u, service_event_name, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + }; + std::vector> messages = + { + serialize_test_message(service_event_name, 5, get_service_event_message_basic_types()[0]), + serialize_test_message(service_event_name, 10, get_service_event_message_basic_types()[1]), + serialize_test_message(service_event_name, 20, get_service_event_message_basic_types()[2]), + serialize_test_message(service_event_name, 25, get_service_event_message_basic_types()[2]), + serialize_test_message(service_event_name, 30, 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> received_service_requests; + + srv_->setup_service(service_name, received_service_requests); + srv_->run_services(); + ASSERT_TRUE(srv_->all_services_ready()); + + play_options_.publish_service_requests = true; + play_options_.service_requests_source = ServiceRequestsSource::CLIENT_INTROSPECTION; + + auto player = + std::make_shared(std::move(reader), storage_options_, play_options_); + + spin_thread_and_wait_for_sent_service_requests_to_finish(player, {service_name}); + + EXPECT_EQ(received_service_requests.size(), 2); + // expected_request is ServiceEventInfo::REQUEST_SENT + const auto expected_request = get_service_event_message_basic_types()[2]->request[0]; + for (const auto & service_request : received_service_requests) { + EXPECT_EQ(service_request->int32_value, expected_request.int32_value) << + service_request->string_value; + EXPECT_EQ(service_request->int64_value, expected_request.int64_value) << + service_request->string_value; + } +} + +TEST_F(RosBag2PlayTestFixture, play_service_events_and_topics) +{ + const std::string topic_1_name = "/topic1"; + const std::string topic_2_name = "/topic2"; + const std::string service_event_1_name = "/test_service1/_service_event"; + const std::string service_event_2_name = "/test_service2/_service_event"; + + auto all_types = std::vector{ + {1u, topic_1_name, "test_msgs/BasicTypes", "", {}, ""}, + {2u, topic_2_name, "test_msgs/BasicTypes", "", {}, ""}, + {3u, service_event_1_name, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + {4u, service_event_2_name, "test_msgs/srv/BasicTypes_Event", "", {}, ""}, + }; + + auto request_received_service_event = get_service_event_message_basic_types()[0]; + std::vector> messages = + { + serialize_test_message(topic_1_name, 10, get_messages_basic_types()[0]), + serialize_test_message(service_event_1_name, 20, request_received_service_event), + serialize_test_message(topic_2_name, 30, get_messages_basic_types()[0]), + serialize_test_message(service_event_2_name, 40, request_received_service_event), + }; + + play_options_.publish_service_requests = false; + + sub_ = std::make_shared(); + sub_->add_subscription(topic_1_name, 1); + sub_->add_subscription(topic_2_name, 1); + sub_->add_subscription(service_event_1_name, 1); + sub_->add_subscription(service_event_2_name, 1); + + 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 player = std::make_shared(std::move(reader), storage_options_, play_options_); + + // Wait for discovery to match publishers with subscribers + ASSERT_TRUE( + sub_->spin_and_wait_for_matched(player->get_list_of_publishers(), std::chrono::seconds(30))); + + auto await_received_messages = sub_->spin_subscriptions(); + + player->play(); + player->wait_for_playback_to_finish(); + + await_received_messages.get(); + + auto replayed_topic1 = sub_->get_received_messages(topic_1_name); + EXPECT_THAT(replayed_topic1, SizeIs(1u)); + auto replayed_topic2 = sub_->get_received_messages(topic_2_name); + EXPECT_THAT(replayed_topic2, SizeIs(1u)); + auto replayed_service_event_1 = + sub_->get_received_messages(service_event_1_name); + EXPECT_THAT(replayed_service_event_1, SizeIs(1u)); + auto replayed_service_event_2 = + sub_->get_received_messages(service_event_2_name); + EXPECT_THAT(replayed_service_event_2, SizeIs(1u)); +} + class RosBag2PlayQosOverrideTestFixture : public RosBag2PlayTestFixture { public: diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_loop.cpp index 78c111f83a..ebd0c58a1d 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( @@ -132,8 +132,8 @@ 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, + rosbag2_transport::PlayOptions play_options{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( diff --git a/rosbag2_transport/test/rosbag2_transport/test_play_next.cpp b/rosbag2_transport/test/rosbag2_transport/test_play_next.cpp index 3c198c1867..81af30549f 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_play_next.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_play_next.cpp @@ -266,18 +266,18 @@ TEST_F(RosBag2PlayTestFixture, play_next_playing_only_filtered_topics) { complex_message1->bool_values = {{true, false, true}}; auto topic_types = std::vector{ - {1u, "topic1", "test_msgs/BasicTypes", "", {}, ""}, - {2u, "topic2", "test_msgs/Arrays", "", {}, ""}, + {1u, "/topic1", "test_msgs/BasicTypes", "", {}, ""}, + {2u, "/topic2", "test_msgs/Arrays", "", {}, ""}, }; std::vector> messages = { - serialize_test_message("topic1", 500, primitive_message1), - serialize_test_message("topic1", 700, primitive_message1), - serialize_test_message("topic1", 900, primitive_message1), - serialize_test_message("topic2", 550, complex_message1), - serialize_test_message("topic2", 750, complex_message1), - serialize_test_message("topic2", 950, complex_message1) + serialize_test_message("/topic1", 500, primitive_message1), + serialize_test_message("/topic1", 700, primitive_message1), + serialize_test_message("/topic1", 900, primitive_message1), + serialize_test_message("/topic2", 550, complex_message1), + serialize_test_message("/topic2", 750, complex_message1), + serialize_test_message("/topic2", 950, complex_message1) }; // Filter allows /topic2, blocks /topic1