Generalize topic filter pipeline utility to be reusable

Improve readability, and mildly improve performance by processing one topic at a time and only creating one new map container, rather than copying to new containers on every operation.

Signed-off-by: Emerson Knapp <[email protected]>

Author: emersonknapp

rosbag2_transport/CMakeLists.txt

@@ -179,11 +179,6 @@ function(create_tests_for_rmw_implementation)
     LINK_LIBS rosbag2_transport
     AMENT_DEPS test_msgs rosbag2_test_common)
 
-  rosbag2_transport_add_gmock(test_topic_filter
-    test/rosbag2_transport/test_topic_filter.cpp
-    INCLUDE_DIRS
-      $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/rosbag2_transport>
-    LINK_LIBS rosbag2_transport)
 endfunction()
 
 if(BUILD_TESTING)
@@ -201,6 +196,11 @@ if(BUILD_TESTING)
   target_include_directories(test_record_options PRIVATE include)
   target_link_libraries(test_record_options ${PROJECT_NAME})
 
+  ament_add_gmock(test_topic_filter
+    test/rosbag2_transport/test_topic_filter.cpp)
+  target_include_directories(test_topic_filter PRIVATE
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/rosbag2_transport>)
+  target_link_libraries(test_topic_filter rosbag2_transport)
 endif()
 
 ament_package()

rosbag2_transport/src/rosbag2_transport/bag_rewrite.cpp

@@ -14,20 +14,16 @@
 
 #include "rosbag2_transport/bag_rewrite.hpp"
 
+#include <map>
 #include <memory>
 #include <string>
 #include <unordered_map>
 #include <unordered_set>
 #include <utility>
 #include <vector>
 
-#include "rosbag2_compression/compression_options.hpp"
-#include "rosbag2_compression/sequential_compression_reader.hpp"
-#include "rosbag2_compression/sequential_compression_writer.hpp"
 #include "rosbag2_cpp/reader.hpp"
-#include "rosbag2_cpp/readers/sequential_reader.hpp"
 #include "rosbag2_cpp/writer.hpp"
-#include "rosbag2_cpp/writers/sequential_writer.hpp"
 #include "rosbag2_transport/reader_writer_factory.hpp"
 
 #include "logging.hpp"
@@ -83,67 +79,29 @@ setup_topic_filtering(
   > & output_bags)
 {
   std::unordered_map<std::string, std::vector<rosbag2_cpp::Writer *>> filtered_outputs;
-  std::unordered_map<std::string, std::string> input_topics;
+  std::map<std::string, std::vector<std::string>> input_topics;
   std::unordered_map<std::string, YAML::Node> input_topics_qos_profiles;
 
-  // Filter inputs
-  {
-    std::unordered_map<std::string, std::unordered_set<std::string>> topic_names_and_types;
-    std::unordered_set<std::string> unknown_types;
-
-    for (const auto & input_bag : input_bags) {
-      auto bag_topics_and_types = input_bag->get_all_topics_and_types();
-      for (const auto & topic_metadata : bag_topics_and_types) {
-        const std::string & topic_name = topic_metadata.name;
-        topic_names_and_types.try_emplace(topic_name);
-        topic_names_and_types[topic_name].insert(topic_metadata.type);
-
-        // Gather all offered qos profiles from all inputs
-        input_topics_qos_profiles.try_emplace(topic_name);
-        YAML::Node & all_offered = input_topics_qos_profiles[topic_name];
-        YAML::Node offered_qos_profiles = YAML::Load(topic_metadata.offered_qos_profiles);
-        for (auto qos : offered_qos_profiles) {
-          all_offered.push_back(qos);
-        }
+  for (const auto & input_bag : input_bags) {
+    auto bag_topics_and_types = input_bag->get_all_topics_and_types();
+    for (const auto & topic_metadata : bag_topics_and_types) {
+      const std::string & topic_name = topic_metadata.name;
+      input_topics.try_emplace(topic_name);
+      input_topics[topic_name].push_back(topic_metadata.type);
+
+      // Gather all offered qos profiles from all inputs
+      input_topics_qos_profiles.try_emplace(topic_name);
+      YAML::Node & all_offered = input_topics_qos_profiles[topic_name];
+      YAML::Node offered_qos_profiles = YAML::Load(topic_metadata.offered_qos_profiles);
+      for (auto qos : offered_qos_profiles) {
+        all_offered.push_back(qos);
       }
     }
-
-    // Filter topics with more than one type
-    for (const auto & [topic_name, topic_types] : topic_names_and_types) {
-      if (topic_types.size() > 1) {
-        ROSBAG2_TRANSPORT_LOG_WARN_STREAM(
-          "Topic '" << topic_name << "' has multiple types from inputs. " <<
-            "Topics must have a single type, skipping topic.");
-      } else {
-        std::string topic_type = *topic_types.begin();
-        input_topics[topic_name] = topic_type;
-      }
-    }
-
-    input_topics = rosbag2_transport::topic_filter::filter_topics_with_known_type(
-      input_topics, unknown_types);
   }
 
-  // Filter to outputs
   for (const auto & [writer, record_options] : output_bags) {
-    auto filtered_topics_and_types = input_topics;
-    if (!record_options.topics.empty()) {
-      std::vector<std::string> expanded_topics;
-      for (const std::string & topic : record_options.topics) {
-        auto expanded_topic = rclcpp::expand_topic_or_service_name(
-          topic, "dummy_node_name", "/", false);
-        expanded_topics.push_back(expanded_topic);
-      }
-      filtered_topics_and_types = rosbag2_transport::topic_filter::filter_topics(
-        expanded_topics, input_topics);
-    }
-    if (!record_options.regex.empty() || !record_options.exclude.empty()) {
-      filtered_topics_and_types = rosbag2_transport::topic_filter::filter_topics_using_regex(
-        filtered_topics_and_types,
-        record_options.regex,
-        record_options.exclude,
-        record_options.all);
-    }
+    rosbag2_transport::TopicFilter topic_filter{record_options};
+    auto filtered_topics_and_types = topic_filter.filter_topics(input_topics);
 
     // Done filtering - set up writer
     for (const auto & [topic_name, topic_type] : filtered_topics_and_types) {

rosbag2_transport/src/rosbag2_transport/recorder.cpp

@@ -90,6 +90,10 @@ Recorder::Recorder(
   RCLCPP_INFO_STREAM(
     get_logger(),
     "Press " << key_str << " for pausing/resuming");
+
+  for (auto & topic : record_options_.topics) {
+    topic = rclcpp::expand_topic_or_service_name(topic, get_name(), get_namespace(), false);
+  }
 }
 
 Recorder::~Recorder()
@@ -193,35 +197,8 @@ std::unordered_map<std::string, std::string>
 Recorder::get_requested_or_available_topics()
 {
   auto all_topics_and_types = this->get_topic_names_and_types();
-  auto filtered_topics_and_types = topic_filter::filter_topics_with_more_than_one_type(
-    all_topics_and_types, record_options_.include_hidden_topics);
-
-  filtered_topics_and_types = topic_filter::filter_topics_with_known_type(
-    filtered_topics_and_types, topic_unknown_types_);
-
-  if (!record_options_.topics.empty()) {
-    // expand specified topics
-    std::vector<std::string> expanded_topics;
-    expanded_topics.reserve(record_options_.topics.size());
-    for (const auto & topic : record_options_.topics) {
-      expanded_topics.push_back(
-        rclcpp::expand_topic_or_service_name(
-          topic, this->get_name(), this->get_namespace(), false));
-    }
-    filtered_topics_and_types = topic_filter::filter_topics(
-      expanded_topics, filtered_topics_and_types);
-  }
-
-  if (record_options_.regex.empty() && record_options_.exclude.empty()) {
-    return filtered_topics_and_types;
-  }
-
-  return topic_filter::filter_topics_using_regex(
-    filtered_topics_and_types,
-    record_options_.regex,
-    record_options_.exclude,
-    record_options_.all
-  );
+  TopicFilter topic_filter{record_options_};
+  return topic_filter.filter_topics(all_topics_and_types);
 }
 
 std::unordered_map<std::string, std::string>