From 64e7deccd85c93296ac277c47f136061dacaa476 Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Fri, 9 Aug 2024 18:41:48 -0700 Subject: [PATCH] [Iron] Add topics with zero message counts to the SQLiteStorage::get_metadata(). (backport #1722) (#1766) * [Humble] Add topics with zero message counts to the SQLiteStorage::get_metadata(). (#1722) (cherry picked from commit 5da1796f506c3182948a5b016a6c828f22f1d35d) # Conflicts: # rosbag2_py/test/test_convert.py # rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp Signed-off-by: Michael Orlov * Address merge conflicts Signed-off-by: Michael Orlov --------- Signed-off-by: Michael Orlov Co-authored-by: Michael Orlov --- .../rosbag2_cpp/test_sequential_reader.cpp | 32 +++++++++++++ .../sqlite_storage.cpp | 47 +++++++++++++++---- 2 files changed, 69 insertions(+), 10 deletions(-) diff --git a/rosbag2_cpp/test/rosbag2_cpp/test_sequential_reader.cpp b/rosbag2_cpp/test/rosbag2_cpp/test_sequential_reader.cpp index 9ec24ac9c2..6548419bd8 100644 --- a/rosbag2_cpp/test/rosbag2_cpp/test_sequential_reader.cpp +++ b/rosbag2_cpp/test/rosbag2_cpp/test_sequential_reader.cpp @@ -246,6 +246,38 @@ TEST_P(ParametrizedTemporaryDirectoryFixture, reader_accepts_bare_file) { EXPECT_THAT(reader.get_metadata().topics_with_message_count, SizeIs(1)); } +TEST_P(ParametrizedTemporaryDirectoryFixture, get_metadata_include_topics_with_zero_messages) { + const auto bag_path = rcpputils::fs::path(temporary_dir_path_) / "bag_with_no_msgs"; + const std::string topic_name = "topic_with_0_messages"; + const auto storage_id = GetParam(); + { + rosbag2_storage::TopicMetadata topic_metadata; + topic_metadata.name = topic_name; + topic_metadata.type = "std_msgs/msg/String"; + + rosbag2_cpp::Writer writer; + rosbag2_storage::StorageOptions options; + options.uri = bag_path.string(); + options.storage_id = storage_id; + writer.open(options); + writer.create_topic(topic_metadata); + } + + rosbag2_storage::MetadataIo metadata_io; + ASSERT_TRUE(metadata_io.metadata_file_exists(bag_path.string())); + auto metadata_from_yaml = metadata_io.read_metadata(bag_path.string()); + auto first_storage = bag_path / metadata_from_yaml.relative_file_paths[0]; + + rosbag2_storage::StorageFactory factory; + rosbag2_storage::StorageOptions options; + options.uri = first_storage.string(); + options.storage_id = storage_id; + auto reader = factory.open_read_only(options); + auto metadata = reader->get_metadata(); + ASSERT_THAT(metadata.topics_with_message_count, SizeIs(1)); + EXPECT_EQ(metadata.topics_with_message_count[0].message_count, 0U); +} + INSTANTIATE_TEST_SUITE_P( BareFileTests, ParametrizedTemporaryDirectoryFixture, 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 4c731b4b34..5be10f1354 100644 --- a/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp +++ b/rosbag2_storage_sqlite3/src/rosbag2_storage_sqlite3/sqlite_storage.cpp @@ -16,6 +16,7 @@ #include +#include #include #include #include @@ -666,6 +667,14 @@ void SqliteStorage::read_metadata() rcutils_time_point_value_t min_time = INT64_MAX; rcutils_time_point_value_t max_time = 0; + if (all_topics_and_types_.empty()) { + fill_topics_and_types(); + } + metadata_.topics_with_message_count.reserve(all_topics_and_types_.size()); + for (const auto & topic_metadata : all_topics_and_types_) { + metadata_.topics_with_message_count.push_back({topic_metadata, 0}); + } + if (database_->field_exists("topics", "offered_qos_profiles")) { if (database_->field_exists("topics", "type_description_hash")) { std::string query = @@ -680,12 +689,21 @@ void SqliteStorage::read_metadata() rcutils_time_point_value_t, std::string, std::string>(); for (auto result : query_results) { - metadata_.topics_with_message_count.push_back( - { - {std::get<0>(result), std::get<1>(result), std::get<2>(result), std::get<6>( - result), std::get<7>(result)}, - static_cast(std::get<3>(result)) + const rosbag2_storage::TopicMetadata topic_metadata{std::get<0>(result), + std::get<1>(result), std::get<2>(result), std::get<6>(result), std::get<7>(result)}; + auto & topics_list = metadata_.topics_with_message_count; + auto it = std::find_if( + topics_list.begin(), topics_list.end(), + [&topic_metadata = topic_metadata](const rosbag2_storage::TopicInformation & topic_info) { + return topic_info.topic_metadata == topic_metadata; }); + if (it != topics_list.end()) { + it->message_count = static_cast(std::get<3>(result)); + } else { + metadata_.topics_with_message_count.push_back( + {topic_metadata, static_cast(std::get<3>(result))} + ); + } metadata_.message_count += std::get<3>(result); min_time = std::get<4>(result) < min_time ? std::get<4>(result) : min_time; @@ -728,12 +746,21 @@ void SqliteStorage::read_metadata() rcutils_time_point_value_t>(); for (auto result : query_results) { - metadata_.topics_with_message_count.push_back( - { - {std::get<0>(result), std::get<1>(result), std::get<2>( - result), "", ""}, - static_cast(std::get<3>(result)) + const rosbag2_storage::TopicMetadata topic_metadata{std::get<0>(result), + std::get<1>(result), std::get<2>(result), "", ""}; + auto & topics_list = metadata_.topics_with_message_count; + auto it = std::find_if( + topics_list.begin(), topics_list.end(), + [&topic_metadata = topic_metadata](const rosbag2_storage::TopicInformation & topic_info) { + return topic_info.topic_metadata == topic_metadata; }); + if (it != topics_list.end()) { + it->message_count = static_cast(std::get<3>(result)); + } else { + metadata_.topics_with_message_count.push_back( + {topic_metadata, static_cast(std::get<3>(result))} + ); + } metadata_.message_count += std::get<3>(result); min_time = std::get<4>(result) < min_time ? std::get<4>(result) : min_time;