ros2GH-23 Get all topics from node and sanitize

rosbag2/include/rosbag2/rosbag2.hpp

@@ -50,6 +50,9 @@ class Rosbag2
   std::map<std::string, std::string> get_topics_with_types(
     const std::vector<std::string> & topic_names, std::shared_ptr<rclcpp::Node> node);
 
+  ROSBAG2_PUBLIC
+  std::map<std::string, std::string> get_all_topics_with_types(std::shared_ptr<rclcpp::Node> node);
+
 private:
   void prepare_publishers(
     std::shared_ptr<Rosbag2Node> node,
@@ -62,6 +65,9 @@ class Rosbag2
     std::shared_ptr<Rosbag2Node> & node,
     const std::string & topic_name, const std::string & topic_type) const;
 
+  std::map<std::string, std::string> sanitize_topics_and_types(
+    std::map<std::string, std::vector<std::string>> topics_and_types);
+
   std::vector<std::shared_ptr<GenericSubscription>> subscriptions_;
   std::map<std::string, std::shared_ptr<GenericPublisher>> publishers_;
 };

rosbag2/src/rosbag2/rosbag2.cpp

@@ -14,10 +14,12 @@
 
 #include "rosbag2/rosbag2.hpp"
 
+#include <algorithm>
 #include <chrono>
 #include <map>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "rcl/graph.h"
@@ -87,35 +89,6 @@ void Rosbag2::record(
   subscriptions_.clear();
 }
 
-std::map<std::string, std::string> Rosbag2::get_topics_with_types(
-  const std::vector<std::string> & topic_names, std::shared_ptr<rclcpp::Node> node)
-{
-  // TODO(Martin-Idel-SI): This is a short sleep to allow the node some time to discover the topic
-  // This should be replaced by an auto-discovery system in the future
-  std::this_thread::sleep_for(std::chrono::milliseconds(100));
-  auto topics = node->get_topic_names_and_types();
-
-  std::map<std::string, std::string> topic_names_and_types;
-  for (const auto & topic_name : topic_names) {
-    std::string complete_topic_name = topic_name;
-    if (topic_name[0] != '/') {
-      complete_topic_name = "/" + topic_name;
-    }
-    auto position = topics.find(complete_topic_name);
-    if (position != topics.end()) {
-      if (position->second.size() > 1) {
-        RCUTILS_LOG_ERROR_NAMED(
-          ROS_PACKAGE_NAME,
-          "Topic '%s' has several types associated. Only topics with one type are supported.",
-          position->first.c_str());
-      } else {
-        topic_names_and_types.insert({position->first, position->second[0]});
-      }
-    }
-  }
-  return topic_names_and_types;
-}
-
 std::shared_ptr<GenericSubscription>
 Rosbag2::create_subscription(
   const std::function<void(std::string)> & after_write_action,
@@ -177,4 +150,67 @@ void Rosbag2::prepare_publishers(
   }
 }
 
+std::map<std::string, std::string> Rosbag2::get_topics_with_types(
+  const std::vector<std::string> & topic_names, std::shared_ptr<rclcpp::Node> node)
+{
+  std::vector<std::string> sanitized_topic_names;
+  std::transform(topic_names.begin(), topic_names.end(), std::back_inserter(sanitized_topic_names),
+    [](std::string topic_name) {
+      return topic_name[0] != '/' ? "/" + topic_name : topic_name;
+    });
+
+  // TODO(Martin-Idel-SI): This is a short sleep to allow the node some time to discover the topic
+  // This should be replaced by an auto-discovery system in the future
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  auto topics_and_types = node->get_topic_names_and_types();
+
+  std::map<std::string, std::vector<std::string>> filtered_topics_and_types;
+  std::remove_copy_if(topics_and_types.begin(), topics_and_types.end(),
+    std::inserter(filtered_topics_and_types, filtered_topics_and_types.end()),
+    [sanitized_topic_names](auto element) {
+      return std::find(sanitized_topic_names.begin(), sanitized_topic_names.end(), element.first) ==
+      sanitized_topic_names.end();
+    });
+
+  return sanitize_topics_and_types(filtered_topics_and_types);
+}
+
+std::map<std::string, std::string>
+Rosbag2::get_all_topics_with_types(std::shared_ptr<rclcpp::Node> node)
+{
+  // TODO(Martin-Idel-SI): This is a short sleep to allow the node some time to discover the topic
+  // This should be replaced by an auto-discovery system in the future
+  std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  return sanitize_topics_and_types(node->get_topic_names_and_types());
+}
+
+std::map<std::string, std::string> Rosbag2::sanitize_topics_and_types(
+  std::map<std::string, std::vector<std::string>> topics_and_types)
+{
+  std::map<std::string, std::vector<std::string>> filtered_topics_and_types;
+  std::remove_copy_if(topics_and_types.begin(),
+    topics_and_types.end(),
+    std::inserter(filtered_topics_and_types, filtered_topics_and_types.end()),
+    [](auto element) {
+      if (element.second.size() > 1) {
+        RCUTILS_LOG_ERROR_NAMED(
+          ROS_PACKAGE_NAME,
+          "Topic '%s' has several types associated. Only topics with one type are supported.",
+          element.first.c_str());
+        return true;
+      }
+      return false;
+    });
+
+  std::map<std::string, std::string> topics_and_types_to_record;
+  std::transform(
+    filtered_topics_and_types.begin(),
+    filtered_topics_and_types.end(),
+    std::inserter(topics_and_types_to_record, topics_and_types_to_record.end()),
+    [](auto element) {
+      return std::make_pair(element.first, element.second[0]);
+    });
+  return topics_and_types_to_record;
+}
+
 }  // namespace rosbag2

rosbag2/test/rosbag2/rosbag2_integration_test.cpp

@@ -59,8 +59,16 @@ class RosBag2IntegrationFixture : public Test
   std::vector<std::shared_ptr<rclcpp::PublisherBase>> publishers_;
 };
 
+TEST_F(RosBag2IntegrationFixture, get_topics_with_types_returns_empty_if_topic_does_not_exist) {
+  create_publisher("string_topic");
+
+  auto topics_and_types = rosbag2_.get_topics_with_types({"/wrong_topic"}, node_);
+
+  ASSERT_THAT(topics_and_types, IsEmpty());
+}
+
 TEST_F(RosBag2IntegrationFixture,
-  get_topic_returns_with_topic_string_if_topic_is_specified_without_slash)
+  get_topics_with_types_returns_with_topic_string_if_topic_is_specified_without_slash)
 {
   create_publisher("string_topic");
 
@@ -71,7 +79,7 @@ TEST_F(RosBag2IntegrationFixture,
 }
 
 TEST_F(RosBag2IntegrationFixture,
-  get_topic_returns_with_topic_string_if_topic_is_specified_with_slash)
+  get_topics_with_types_returns_with_topic_string_if_topic_is_specified_with_slash)
 {
   create_publisher("string_topic");
 
@@ -81,7 +89,7 @@ TEST_F(RosBag2IntegrationFixture,
   EXPECT_THAT(topics_and_types.begin()->second, StrEq("std_msgs/String"));
 }
 
-TEST_F(RosBag2IntegrationFixture, returns_multiple_topics_for_multiple_inputs)
+TEST_F(RosBag2IntegrationFixture, get_topics_with_types_returns_multiple_topics_for_multiple_inputs)
 {
   std::string first_topic("/string_topic");
   std::string second_topic("/other_topic");
@@ -98,10 +106,24 @@ TEST_F(RosBag2IntegrationFixture, returns_multiple_topics_for_multiple_inputs)
   EXPECT_THAT(topics_and_types.find(second_topic)->second, StrEq("std_msgs/String"));
 }
 
-TEST_F(RosBag2IntegrationFixture, get_topic_returns_empty_if_topic_does_not_exist) {
-  create_publisher("string_topic");
+TEST_F(RosBag2IntegrationFixture, get_all_topics_with_types_returns_all_topics)
+{
+  std::string first_topic("/string_topic");
+  std::string second_topic("/other_topic");
+  std::string third_topic("/wrong_topic");
 
-  auto topics_and_types = rosbag2_.get_topics_with_types({"/wrong_topic"}, node_);
+  create_publisher(first_topic);
+  create_publisher(second_topic);
+  create_publisher(third_topic);
 
-  ASSERT_THAT(topics_and_types, IsEmpty());
+  auto topics_and_types = rosbag2_.get_all_topics_with_types(node_);
+
+  ASSERT_THAT(topics_and_types, SizeIs(5));
+  EXPECT_THAT(topics_and_types.find(first_topic)->second, StrEq("std_msgs/String"));
+  EXPECT_THAT(topics_and_types.find(second_topic)->second, StrEq("std_msgs/String"));
+  EXPECT_THAT(topics_and_types.find(third_topic)->second, StrEq("std_msgs/String"));
+  // The latter two topics can usually be found on any node, so they should be subscribed here
+  EXPECT_THAT(topics_and_types.find("/clock")->second, StrEq("rosgraph_msgs/Clock"));
+  EXPECT_THAT(
+    topics_and_types.find("/parameter_events")->second, StrEq("rcl_interfaces/ParameterEvent"));
 }