From 9ec61eaf72088d40dfde85d6c4972d491676fd68 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Mon, 2 Dec 2024 07:52:33 -0800 Subject: [PATCH] [jazzy] Add computation of size contribution to info verb (backport #1726) (#1872) * Add computation of size contribution to info verb (#1726) * Add optional computation of size contribution to info verb Signed-off-by: Nicola Loi * Update rosbag2_cpp/src/rosbag2_cpp/info.cpp Co-authored-by: Michael Orlov Signed-off-by: Nicola Loi * Fixes for review and failed tests - Also update rosbag2_tests Signed-off-by: Nicola Loi * Support services' size - Also add new test and update design doc Signed-off-by: Nicola Loi * Fix style divergence Signed-off-by: Nicola Loi * Apply suggestions from code review Co-authored-by: Michael Orlov Signed-off-by: Nicola Loi * Update timestamp check for new ros bag info test Signed-off-by: Nicola Loi --------- Signed-off-by: Nicola Loi Co-authored-by: Michael Orlov (cherry picked from commit 22148a7a8efa5e465eb8c88035d29e02c332b327) * Make PR ABI/API compatible - Made "compute_messages_size_contribution(..)"" as non-virtual method - Make "format_bag_meta_data(..)" APi compatible by moving "messages_size" argument to the end. Signed-off-by: Michael Orlov --------- Signed-off-by: Michael Orlov Co-authored-by: Nicola Loi Co-authored-by: Michael Orlov --- docs/design/rosbag2_record_replay_service.md | 14 +++--- ros2bag/ros2bag/verb/info.py | 26 ++--------- rosbag2_cpp/include/rosbag2_cpp/info.hpp | 4 ++ rosbag2_cpp/src/rosbag2_cpp/info.cpp | 18 ++++++++ rosbag2_py/rosbag2_py/_info.pyi | 4 +- rosbag2_py/src/rosbag2_py/_info.cpp | 44 +++++++++++++++---- .../src/rosbag2_py/format_bag_metadata.cpp | 35 ++++++++++++--- .../src/rosbag2_py/format_bag_metadata.hpp | 6 ++- .../src/rosbag2_py/format_service_info.cpp | 39 +++++++++++++++- .../src/rosbag2_py/format_service_info.hpp | 5 ++- .../test_rosbag2_info_end_to_end.cpp | 42 +++++++++++++++--- 11 files changed, 184 insertions(+), 53 deletions(-) diff --git a/docs/design/rosbag2_record_replay_service.md b/docs/design/rosbag2_record_replay_service.md index 72ea4dec53..48ce2a5248 100644 --- a/docs/design/rosbag2_record_replay_service.md +++ b/docs/design/rosbag2_record_replay_service.md @@ -78,7 +78,7 @@ A new parameter is added. - `-v` or `--verbose` - This parameter only affects the service event topic. if `-v` or `--verbose` is set, the info command shows the number of requests and responses for each service based on the service event. Otherwise, only show the number of service events. Note that parsing the number of request and response need spent time. The duration of the parsing is related to the number of recorded service events. + If `-v` or `--verbose` is set, the info command shows two additional pieces of information: the size contribution of topics and services, and the number of requests and responses for each service based on the service event. Otherwise, only show the message count of the topics and the general number of service events. Note that parsing the messages need spent time. The duration of the parsing is related to the number of recorderd topic messages and service events. Without `-v` or `--verbose` parameter, info command shows as below example. @@ -111,15 +111,15 @@ Duration: 15.59s Start: May 19 2023 13:22:25.340 (1684473745.340) End: May 19 2023 13:22:40.400 (1684473760.400) Messages: 60 -Topic information: Topic: /chatter | Type: std_msgs/msg/String | Count: 16 | Serialization Format: cdr - Topic: /events/write_split | Type: rosbag2_interfaces/msg/WriteSplitEvent | Count: 0 | Serialization Format: cdr - Topic: /parameter_events | Type: rcl_interfaces/msg/ParameterEvent | Count: 0 | Serialization Format: cdr - Topic: /rosout | Type: rcl_interfaces/msg/Log | Count: 44 | Serialization Format: cdr +Topic information: Topic: /chatter | Type: std_msgs/msg/String | Count: 16 | Size Contribution XX xxB | Serialization Format: cdr + Topic: /events/write_split | Type: rosbag2_interfaces/msg/WriteSplitEvent | Count: 0 | Size Contribution 0 B | Serialization Format: cdr + Topic: /parameter_events | Type: rcl_interfaces/msg/ParameterEvent | Count: 0 | Size Contribution 0 B | Serialization Format: cdr + Topic: /rosout | Type: rcl_interfaces/msg/Log | Count: 44 | Size Contribution XX xxB | Serialization Format: cdr >>>> The below is new added items for service <<<<<<< Service : XX <== The number of service -Service information: Service: /xxx/xxx | Type: xxx/xxx/xxx | Request Count: XX | Response Count: XX | Serialization Format: XX - Service: /xxx/xxx | Type: xxx/xxx/xxx | Request Count: XX | Response Count: XX | Serialization Format: XX +Service information: Service: /xxx/xxx | Type: xxx/xxx/xxx | Request Count: XX | Response Count: XX | Size Contribution XX xxB | Serialization Format: XX + Service: /xxx/xxx | Type: xxx/xxx/xxx | Request Count: XX | Response Count: XX | Size Contribution XX xxB | Serialization Format: XX ``` ### Expand the 'play' command diff --git a/ros2bag/ros2bag/verb/info.py b/ros2bag/ros2bag/verb/info.py index 160f128c8c..910cf64079 100644 --- a/ros2bag/ros2bag/verb/info.py +++ b/ros2bag/ros2bag/verb/info.py @@ -31,33 +31,15 @@ def add_arguments(self, parser, cli_name): # noqa: D102 help='Display request/response information for services' ) - def _is_service_event_topic(self, topic_name, topic_type) -> bool: - - service_event_type_middle = '/srv/' - service_event_type_postfix = '_Event' - - if (service_event_type_middle not in topic_type - or not topic_type.endswith(service_event_type_postfix)): - return False - - service_event_topic_postfix = '/_service_event' - if not topic_name.endswith(service_event_topic_postfix): - return False - - return True - def main(self, *, args): # noqa: D102 if args.topic_name and args.verbose: print("Warning! You have set both the '-t' and '-v' parameters. The '-t' parameter " 'will be ignored.') + m = Info().read_metadata(args.bag_path, args.storage) if args.verbose: - Info().read_metadata_and_output_service_verbose(args.bag_path, args.storage) + Info().print_output_verbose(args.bag_path, m) else: - m = Info().read_metadata(args.bag_path, args.storage) if args.topic_name: - for topic_info in m.topics_with_message_count: - if not self._is_service_event_topic(topic_info.topic_metadata.name, - topic_info.topic_metadata.type): - print(topic_info.topic_metadata.name) + Info().print_output_topic_name_only(m) else: - print(m) + Info().print_output(m) diff --git a/rosbag2_cpp/include/rosbag2_cpp/info.hpp b/rosbag2_cpp/include/rosbag2_cpp/info.hpp index a251598ea5..6826a79c77 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/info.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/info.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rosbag2_cpp/visibility_control.hpp" @@ -45,6 +46,9 @@ class ROSBAG2_CPP_PUBLIC Info virtual std::vector> read_service_info( const std::string & uri, const std::string & storage_id = ""); + + std::unordered_map compute_messages_size_contribution( + const std::string & uri, const std::string & storage_id = ""); }; } // namespace rosbag2_cpp diff --git a/rosbag2_cpp/src/rosbag2_cpp/info.cpp b/rosbag2_cpp/src/rosbag2_cpp/info.cpp index 74e62f4c7e..87ec2bc5f1 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/info.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/info.cpp @@ -165,4 +165,22 @@ std::vector> Info::read_service_info( return ret_service_info; } +std::unordered_map Info::compute_messages_size_contribution( + const std::string & uri, const std::string & storage_id) +{ + rosbag2_storage::StorageFactory factory; + auto storage = factory.open_read_only({uri, storage_id}); + if (!storage) { + throw std::runtime_error("No plugin detected that could open file " + uri); + } + + std::unordered_map messages_size; + while (storage->has_next()) { + auto bag_msg = storage->read_next(); + messages_size[bag_msg->topic_name] += bag_msg->serialized_data->buffer_length; + } + + return messages_size; +} + } // namespace rosbag2_cpp diff --git a/rosbag2_py/rosbag2_py/_info.pyi b/rosbag2_py/rosbag2_py/_info.pyi index cee3f18c1c..8a7eb90c8a 100644 --- a/rosbag2_py/rosbag2_py/_info.pyi +++ b/rosbag2_py/rosbag2_py/_info.pyi @@ -2,5 +2,7 @@ import rosbag2_py._storage class Info: def __init__(self) -> None: ... + def print_output(self, arg0: rosbag2_py._storage.BagMetadata) -> None: ... + def print_output_topic_name_only(self, arg0: rosbag2_py._storage.BagMetadata) -> None: ... + def print_output_verbose(self, arg0: str, arg1: rosbag2_py._storage.BagMetadata) -> None: ... def read_metadata(self, arg0: str, arg1: str) -> rosbag2_py._storage.BagMetadata: ... - def read_metadata_and_output_service_verbose(self, arg0: str, arg1: str) -> None: ... diff --git a/rosbag2_py/src/rosbag2_py/_info.cpp b/rosbag2_py/src/rosbag2_py/_info.cpp index 6c73a70a01..316d15af82 100644 --- a/rosbag2_py/src/rosbag2_py/_info.cpp +++ b/rosbag2_py/src/rosbag2_py/_info.cpp @@ -19,6 +19,7 @@ #include "format_bag_metadata.hpp" #include "format_service_info.hpp" #include "rosbag2_cpp/info.hpp" +#include "rosbag2_cpp/service_utils.hpp" #include "rosbag2_storage/bag_metadata.hpp" #include "pybind11.hpp" @@ -41,12 +42,27 @@ class Info return info_->read_metadata(uri, storage_id); } - void read_metadata_and_output_service_verbose( - const std::string & uri, - const std::string & storage_id) + void print_output(const rosbag2_storage::BagMetadata & metadata_info) { - auto metadata_info = read_metadata(uri, storage_id); + // Output formatted metadata + std::cout << format_bag_meta_data(metadata_info) << std::endl; + } + + void print_output_topic_name_only(const rosbag2_storage::BagMetadata & metadata_info) + { + for (const auto & topic_info : metadata_info.topics_with_message_count) { + if (!rosbag2_cpp::is_service_event_topic( + topic_info.topic_metadata.name, + topic_info.topic_metadata.type)) + { + std::cout << topic_info.topic_metadata.name << std::endl; + } + } + } + void print_output_verbose( + const std::string & uri, const rosbag2_storage::BagMetadata & metadata_info) + { std::vector> all_services_info; for (auto & file_info : metadata_info.files) { auto services_info = info_->read_service_info( @@ -60,9 +76,19 @@ class Info } } + std::unordered_map messages_size = {}; + for (const auto & file_info : metadata_info.files) { + auto messages_size_tmp = info_->compute_messages_size_contribution( + uri + "/" + file_info.path, + metadata_info.storage_identifier); + for (const auto & topic_size_tmp : messages_size_tmp) { + messages_size[topic_size_tmp.first] += topic_size_tmp.second; + } + } + // Output formatted metadata and service info - std::cout << format_bag_meta_data(metadata_info, true); - std::cout << format_service_info(all_services_info) << std::endl; + std::cout << format_bag_meta_data(metadata_info, true, true, messages_size); + std::cout << format_service_info(all_services_info, messages_size, true) << std::endl; } protected: @@ -77,7 +103,7 @@ PYBIND11_MODULE(_info, m) { pybind11::class_(m, "Info") .def(pybind11::init()) .def("read_metadata", &rosbag2_py::Info::read_metadata) - .def( - "read_metadata_and_output_service_verbose", - &rosbag2_py::Info::read_metadata_and_output_service_verbose); + .def("print_output", &rosbag2_py::Info::print_output) + .def("print_output_topic_name_only", &rosbag2_py::Info::print_output_topic_name_only) + .def("print_output_verbose", &rosbag2_py::Info::print_output_verbose); } diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp index 6622bd611b..cda904e463 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.cpp @@ -112,6 +112,8 @@ void format_file_paths( void format_topics_with_type( const std::vector & topics, + const std::unordered_map & messages_size, + bool verbose, std::stringstream & info_stream, int indentation_spaces) { @@ -121,10 +123,18 @@ void format_topics_with_type( } auto print_topic_info = - [&info_stream](const rosbag2_storage::TopicInformation & ti) -> void { + [&info_stream, &messages_size, verbose](const rosbag2_storage::TopicInformation & ti) -> void { info_stream << "Topic: " << ti.topic_metadata.name << " | "; info_stream << "Type: " << ti.topic_metadata.type << " | "; info_stream << "Count: " << ti.message_count << " | "; + if (verbose) { + uint64_t topic_size = 0; + auto topic_size_iter = messages_size.find(ti.topic_metadata.name); + if (topic_size_iter != messages_size.end()) { + topic_size = topic_size_iter->second; + } + info_stream << "Size Contribution: " << format_file_size(topic_size) << " | "; + } info_stream << "Serialization Format: " << ti.topic_metadata.serialization_format; info_stream << std::endl; }; @@ -199,6 +209,8 @@ std::vector> filter_service_event_topic( void format_service_with_type( const std::vector> & services, + const std::unordered_map & messages_size, + bool verbose, std::stringstream & info_stream, int indentation_spaces) { @@ -208,10 +220,20 @@ void format_service_with_type( } auto print_service_info = - [&info_stream](const std::shared_ptr & si) -> void { + [&info_stream, &messages_size, verbose]( + const std::shared_ptr & si) -> void { info_stream << "Service: " << si->service_metadata.name << " | "; info_stream << "Type: " << si->service_metadata.type << " | "; info_stream << "Event Count: " << si->event_message_count << " | "; + if (verbose) { + uint64_t service_size = 0; + auto service_size_iter = messages_size.find( + rosbag2_cpp::service_name_to_service_event_topic_name(si->service_metadata.name)); + if (service_size_iter != messages_size.end()) { + service_size = service_size_iter->second; + } + info_stream << "Size Contribution: " << format_file_size(service_size) << " | "; + } info_stream << "Serialization Format: " << si->service_metadata.serialization_format; info_stream << std::endl; }; @@ -231,7 +253,9 @@ namespace rosbag2_py std::string format_bag_meta_data( const rosbag2_storage::BagMetadata & metadata, - bool only_topic) + bool verbose, + bool only_topic, + const std::unordered_map & messages_size) { auto start_time = metadata.starting_time.time_since_epoch(); auto end_time = start_time + metadata.duration; @@ -264,13 +288,14 @@ std::string format_bag_meta_data( std::endl; info_stream << "Topic information: "; format_topics_with_type( - metadata.topics_with_message_count, info_stream, indentation_spaces); + metadata.topics_with_message_count, messages_size, verbose, info_stream, indentation_spaces); if (!only_topic) { info_stream << "Service: " << service_info_list.size() << std::endl; info_stream << "Service information: "; if (!service_info_list.empty()) { - format_service_with_type(service_info_list, info_stream, indentation_spaces + 2); + format_service_with_type( + service_info_list, messages_size, verbose, info_stream, indentation_spaces + 2); } } diff --git a/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp b/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp index dabd871f93..cbaccb803b 100644 --- a/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp +++ b/rosbag2_py/src/rosbag2_py/format_bag_metadata.hpp @@ -16,6 +16,7 @@ #define ROSBAG2_PY__FORMAT_BAG_METADATA_HPP_ #include +#include #include "rosbag2_storage/bag_metadata.hpp" @@ -23,7 +24,10 @@ namespace rosbag2_py { std::string format_bag_meta_data( - const rosbag2_storage::BagMetadata & metadata, bool only_topic = false); + const rosbag2_storage::BagMetadata & metadata, + bool verbose = false, + bool only_topic = false, + const std::unordered_map & messages_size = {}); } // namespace rosbag2_py diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.cpp b/rosbag2_py/src/rosbag2_py/format_service_info.cpp index a14ee89e01..3794259158 100644 --- a/rosbag2_py/src/rosbag2_py/format_service_info.cpp +++ b/rosbag2_py/src/rosbag2_py/format_service_info.cpp @@ -15,13 +15,38 @@ #include #include "format_service_info.hpp" +#include "rosbag2_cpp/service_utils.hpp" + +namespace +{ + +std::string format_file_size(uint64_t file_size) +{ + double size = static_cast(file_size); + static const char * units[] = {"B", "KiB", "MiB", "GiB", "TiB"}; + double reference_number_bytes = 1024; + int index = 0; + while (size >= reference_number_bytes && index < 4) { + size /= reference_number_bytes; + index++; + } + + std::stringstream rounded_size; + int size_format_precision = index == 0 ? 0 : 1; + rounded_size << std::setprecision(size_format_precision) << std::fixed << size; + return rounded_size.str() + " " + units[index]; +} + +} // namespace namespace rosbag2_py { std::string format_service_info( - std::vector> & service_info_list) + std::vector> & service_info_list, + const std::unordered_map & messages_size, + bool verbose) { std::stringstream info_stream; const std::string service_info_string = "Service information: "; @@ -34,11 +59,21 @@ format_service_info( } auto print_service_info = - [&info_stream](const std::shared_ptr & si) -> void { + [&info_stream, &messages_size, verbose]( + const std::shared_ptr & si) -> void { info_stream << "Service: " << si->name << " | "; info_stream << "Type: " << si->type << " | "; info_stream << "Request Count: " << si->request_count << " | "; info_stream << "Response Count: " << si->response_count << " | "; + if (verbose) { + uint64_t service_size = 0; + auto service_size_iter = messages_size.find( + rosbag2_cpp::service_name_to_service_event_topic_name(si->name)); + if (service_size_iter != messages_size.end()) { + service_size = service_size_iter->second; + } + info_stream << "Size Contribution: " << format_file_size(service_size) << " | "; + } info_stream << "Serialization Format: " << si->serialization_format; info_stream << std::endl; }; diff --git a/rosbag2_py/src/rosbag2_py/format_service_info.hpp b/rosbag2_py/src/rosbag2_py/format_service_info.hpp index d5cb5fc788..2feb1432cf 100644 --- a/rosbag2_py/src/rosbag2_py/format_service_info.hpp +++ b/rosbag2_py/src/rosbag2_py/format_service_info.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "rosbag2_cpp/info.hpp" @@ -25,7 +26,9 @@ namespace rosbag2_py { std::string format_service_info( - std::vector> & service_info); + std::vector> & service_info, + const std::unordered_map & messages_size = {}, + bool verbose = false); } // namespace rosbag2_py diff --git a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp index 29128db59d..61808d1c9c 100644 --- a/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp +++ b/rosbag2_tests/test/rosbag2_tests/test_rosbag2_info_end_to_end.cpp @@ -130,27 +130,59 @@ TEST_P(InfoEndToEndTestFixture, info_with_verbose_option_end_to_end_test) { EXPECT_THAT( output, HasSubstr( "Topic: /events/write_split | Type: rosbag2_interfaces/msg/WriteSplitEvent | Count: 0 | " - "Serialization Format: cdr\n")); + "Size Contribution: 0 B | Serialization Format: cdr\n")); EXPECT_THAT( output, HasSubstr( "Topic: /test_topic1 | Type: test_msgs/msg/Strings | Count: 1 | " - "Serialization Format: cdr\n")); + "Size Contribution: 217 B | Serialization Format: cdr\n")); EXPECT_THAT( output, HasSubstr( "Topic: /test_topic2 | Type: test_msgs/msg/Strings | Count: 1 | " - "Serialization Format: cdr\n")); + "Size Contribution: 217 B | Serialization Format: cdr\n")); EXPECT_THAT(output, HasSubstr("Service: 2\n")); EXPECT_THAT( output, HasSubstr( "Service: /test_service1 | Type: test_msgs/srv/BasicTypes | Request Count: 2 | " - "Response Count: 2 | Serialization Format: cdr\n")); + "Response Count: 2 | Size Contribution: 434 B | Serialization Format: cdr\n")); EXPECT_THAT( output, HasSubstr( "Service: /test_service2 | Type: test_msgs/srv/BasicTypes | Request Count: 2 | " - "Response Count: 2 | Serialization Format: cdr\n")); + "Response Count: 2 | Size Contribution: 434 B | Serialization Format: cdr\n")); +} + +TEST_P(InfoEndToEndTestFixture, info_basic_types_and_arrays_with_verbose_option_end_to_end_test) { + internal::CaptureStdout(); + auto exit_code = execute_and_wait_until_completion( + "ros2 bag info cdr_test --verbose", bags_path_); + std::string output = internal::GetCapturedStdout(); + auto expected_storage = GetParam(); + auto expected_file = rosbag2_test_common::bag_filename_for_storage_id("cdr_test_0", GetParam()); + std::string expected_ros_distro = "unknown"; + + EXPECT_THAT(exit_code, Eq(EXIT_SUCCESS)); + // The bag size depends on the os and is not asserted, the time is asserted time zone independent + EXPECT_THAT( + output, ContainsRegex( + "\nFiles: " + expected_file + + "\nBag size: .*B" + "\nStorage id: " + expected_storage + + "\nROS Distro: " + expected_ros_distro + + "\nDuration: 0.151137181s" + "\nStart: Apr .+ 2020 .*:.*:36.763032325 \\(1586406456.763032325\\)" + "\nEnd: Apr .+ 2020 .*:.*:36.914169506 \\(1586406456.914169506\\)" + "\nMessages: 7" + "\nTopic information: ")); + EXPECT_THAT( + output, HasSubstr( + "Topic: /test_topic | Type: test_msgs/msg/BasicTypes | Count: 3 | " + "Size Contribution: 156 B | Serialization Format: cdr\n")); + EXPECT_THAT( + output, HasSubstr( + "Topic: /array_topic | Type: test_msgs/msg/Arrays | Count: 4 | " + "Size Contribution: 2.7 KiB | Serialization Format: cdr")); } // TODO(Martin-Idel-SI): Revisit exit code non-zero here, gracefully should be exit code zero