diff --git a/rosbag2_transport/include/rosbag2_transport/topic_filter.hpp b/rosbag2_transport/include/rosbag2_transport/topic_filter.hpp index 4335fbcd93..e146302bcd 100644 --- a/rosbag2_transport/include/rosbag2_transport/topic_filter.hpp +++ b/rosbag2_transport/include/rosbag2_transport/topic_filter.hpp @@ -62,6 +62,7 @@ class ROSBAG2_TRANSPORT_PUBLIC TopicFilter bool allow_unknown_types_ = false; std::unordered_set already_warned_unknown_types_; rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_; + std::unordered_map take_topics_cache_; }; } // namespace rosbag2_transport diff --git a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp index a09f6b70c3..273f928527 100644 --- a/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp +++ b/rosbag2_transport/src/rosbag2_transport/topic_filter.cpp @@ -75,7 +75,7 @@ topic_is_unpublished( const std::string & topic_name, rclcpp::node_interfaces::NodeGraphInterface & node_graph) { auto publishers_info = node_graph.get_publishers_info_by_topic(topic_name); - return publishers_info.size() == 0; + return publishers_info.empty(); } bool @@ -83,7 +83,7 @@ is_leaf_topic( const std::string & topic_name, rclcpp::node_interfaces::NodeGraphInterface & node_graph) { auto subscriptions_info = node_graph.get_subscriptions_info_by_topic(topic_name); - return subscriptions_info.size() == 0; + return subscriptions_info.empty(); } } // namespace @@ -94,21 +94,31 @@ TopicFilter::TopicFilter( RecordOptions record_options, rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph, bool allow_unknown_types) -: record_options_(record_options), +: record_options_(std::move(record_options)), allow_unknown_types_(allow_unknown_types), node_graph_(node_graph) {} -TopicFilter::~TopicFilter() -{} +TopicFilter::~TopicFilter() = default; std::unordered_map TopicFilter::filter_topics( const std::map> & topic_names_and_types) { std::unordered_map filtered_topics; for (const auto & [topic_name, topic_types] : topic_names_and_types) { - if (take_topic(topic_name, topic_types)) { - filtered_topics.insert(std::make_pair(topic_name, topic_types[0])); + // Check take_topics_cache_ first to avoid performance burden when discovery thread running + auto take_topics_cache_it = take_topics_cache_.find(topic_name); + if (take_topics_cache_it != take_topics_cache_.end()) { + if (take_topics_cache_it->second) { + filtered_topics.insert(std::make_pair(topic_name, topic_types[0])); + } + } else { + if (take_topic(topic_name, topic_types)) { + filtered_topics.insert(std::make_pair(topic_name, topic_types[0])); + take_topics_cache_[topic_name] = true; + } else { + take_topics_cache_[topic_name] = false; + } } } return filtered_topics; @@ -117,6 +127,11 @@ std::unordered_map TopicFilter::filter_topics( bool TopicFilter::take_topic( const std::string & topic_name, const std::vector & topic_types) { + if (!has_single_type(topic_name, topic_types)) { + return false; + } + const std::string & topic_type = topic_types[0]; + if (!record_options_.include_unpublished_topics && node_graph_ && topic_is_unpublished(topic_name, *node_graph_)) { @@ -147,10 +162,6 @@ bool TopicFilter::take_topic( return false; } - if (!has_single_type(topic_name, topic_types)) { - return false; - } - if (!record_options_.include_hidden_topics && topic_is_hidden(topic_name)) { RCUTILS_LOG_WARN_ONCE_NAMED( ROSBAG2_TRANSPORT_PACKAGE_NAME, @@ -158,7 +169,6 @@ bool TopicFilter::take_topic( return false; } - const std::string & topic_type = topic_types[0]; if (!allow_unknown_types_ && !type_is_known(topic_name, topic_type)) { return false; }