ros2GH-23 Improve readability of sanitizing topics and types

bosch-io · Sep 5, 2018 · 9ab0ee2 · 9ab0ee2
1 parent 9bb86b8
commit 9ab0ee2
Show file tree

Hide file tree

Showing 3 changed files with 67 additions and 55 deletions.
diff --git a/rosbag2/src/rosbag2/rosbag2_node.cpp b/rosbag2/src/rosbag2/rosbag2_node.cpp
@@ -65,25 +65,25 @@ std::map<std::string, std::string> Rosbag2Node::get_topics_with_types(
   const std::vector<std::string> & topic_names)
 {
   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;
-    });
+  for (const auto & topic_name : topic_names) {
+    sanitized_topic_names.push_back(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 = this->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);
+  for (const auto & topic_and_type : topics_and_types) {
+    if (std::find(sanitized_topic_names.begin(), sanitized_topic_names.end(),
+      topic_and_type.first) != sanitized_topic_names.end())
+    {
+      filtered_topics_and_types.insert(topic_and_type);
+    }
+  }
+
+  return reduce_multiple_types_to_one(filter_topics_with_wrong_types(filtered_topics_and_types));
 }
 
 std::map<std::string, std::string>
@@ -92,46 +92,46 @@ Rosbag2Node::get_all_topics_with_types()
   // 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(this->get_topic_names_and_types());
+  return reduce_multiple_types_to_one(
+    filter_topics_with_wrong_types(this->get_topic_names_and_types()));
+}
+
+bool type_is_of_incorrect_form(const std::string & type)
+{
+  char type_separator = '/';
+  auto sep_position_back = type.find_last_of(type_separator);
+  auto sep_position_front = type.find_first_of(type_separator);
+  return sep_position_back == std::string::npos ||
+         sep_position_back != sep_position_front ||
+         sep_position_back == 0 ||
+         sep_position_back == type.length() - 1;
 }
 
-std::map<std::string, std::string> Rosbag2Node::sanitize_topics_and_types(
+std::map<std::string, std::vector<std::string>> Rosbag2Node::filter_topics_with_wrong_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) {
-        ROSBAG2_LOG_ERROR_STREAM("Topic '" << element.first <<
+  for (const auto & topic_and_type : topics_and_types) {
+    if (topic_and_type.second.size() > 1) {
+      ROSBAG2_LOG_ERROR_STREAM("Topic '" << topic_and_type.first <<
         "' has several types associated. Only topics with one type are supported");
-        return true;
-      } else {
-        char type_separator = '/';
-        auto sep_position_back = element.second[0].find_last_of(type_separator);
-        auto sep_position_front = element.second[0].find_first_of(type_separator);
-        if (sep_position_back == std::string::npos ||
-        sep_position_back != sep_position_front ||
-        sep_position_back == 0 ||
-        sep_position_back == element.second[0].length() - 1)
-        {
-          ROSBAG2_LOG_ERROR_STREAM("Topic '" << element.first <<
-          "' has non-ROS type '" << element.second[0] << "'. Only ROS topics are supported.");
-          return true;
-        }
-        return false;
-      }
-    });
+    } else if (type_is_of_incorrect_form(topic_and_type.second[0])) {
+      ROSBAG2_LOG_ERROR_STREAM("Topic '" << topic_and_type.first << "' has non-ROS type '" <<
+        topic_and_type.second[0] << "'. Only ROS topics are supported.");
+    } else {
+      filtered_topics_and_types.insert(topic_and_type);
+    }
+  }
+  return filtered_topics_and_types;
+}
 
+std::map<std::string, std::string> Rosbag2Node::reduce_multiple_types_to_one(
+  std::map<std::string, std::vector<std::string>> topics_and_types)
+{
   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]);
-    });
+  for (const auto & topic_and_types : topics_and_types) {
+    topics_and_types_to_record.insert({topic_and_types.first, topic_and_types.second[0]});
+  }
   return topics_and_types_to_record;
 }
 

diff --git a/rosbag2/src/rosbag2/rosbag2_node.hpp b/rosbag2/src/rosbag2/rosbag2_node.hpp
@@ -48,7 +48,10 @@ class Rosbag2Node : public rclcpp::Node
 
   std::map<std::string, std::string> get_all_topics_with_types();
 
-  std::map<std::string, std::string> sanitize_topics_and_types(
+  std::map<std::string, std::vector<std::string>> filter_topics_with_wrong_types(
+    std::map<std::string, std::vector<std::string>> topics_and_types);
+
+  std::map<std::string, std::string> reduce_multiple_types_to_one(
     std::map<std::string, std::vector<std::string>> topics_and_types);
 };
 

diff --git a/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp b/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp
@@ -179,29 +179,38 @@ TEST_F(RosBag2NodeFixture, get_all_topics_with_types_returns_all_topics)
     topics_and_types.find("/parameter_events")->second, StrEq("rcl_interfaces/ParameterEvent"));
 }
 
-TEST_F(RosBag2NodeFixture, sanitize_topics_and_types_deletes_topics_with_multiple_types)
-{
+TEST_F(RosBag2NodeFixture, filter_topics_with_wrong_type_deletes_topics_with_multiple_types) {
   std::map<std::string, std::vector<std::string>> topics_and_types = {
     {"/topic", {"package1/type1"}},
     {"/topic2", {"package1/type1", "package2/type2"}}};
 
-  auto sanitized_topics_and_types = node_->sanitize_topics_and_types(topics_and_types);
+  auto filtered_topics_and_types = node_->filter_topics_with_wrong_types(topics_and_types);
 
-  ASSERT_THAT(sanitized_topics_and_types, SizeIs(1));
-  EXPECT_THAT(sanitized_topics_and_types.find("/topic")->second, StrEq("package1/type1"));
+  ASSERT_THAT(filtered_topics_and_types, SizeIs(1));
+  EXPECT_THAT(filtered_topics_and_types.find("/topic")->second[0], StrEq("package1/type1"));
 }
 
-TEST_F(RosBag2NodeFixture, sanitize_topics_and_types_deletes_topics_with_wrong_type_format)
-{
+TEST_F(RosBag2NodeFixture, filter_topics_with_wrong_type_deletes_topics_with_wrong_type_format) {
   std::map<std::string, std::vector<std::string>> topics_and_types = {
     {"/topic", {"package1/type1"}},
     {"/topic2", {"package1/"}},
     {"/topic3", {"package1/package2/type"}},
     {"/topic4", {"/type"}}
   };
 
-  auto sanitized_topics_and_types = node_->sanitize_topics_and_types(topics_and_types);
+  auto filtered_topics_and_types = node_->filter_topics_with_wrong_types(topics_and_types);
+
+  ASSERT_THAT(filtered_topics_and_types, SizeIs(1));
+  EXPECT_THAT(filtered_topics_and_types.find("/topic")->second[0], StrEq("package1/type1"));
+}
+
+TEST_F(RosBag2NodeFixture, reduce_multiple_types_to_one) {
+  std::map<std::string, std::vector<std::string>> topics_and_types = {
+    {"/topic", {"package1/type1"}},
+    {"/topic2", {"package1/type1", "package2/type2"}}};
+
+  auto reduced_types = node_->reduce_multiple_types_to_one(topics_and_types);
 
-  ASSERT_THAT(sanitized_topics_and_types, SizeIs(1));
-  EXPECT_THAT(sanitized_topics_and_types.find("/topic")->second, StrEq("package1/type1"));
+  ASSERT_THAT(reduced_types, SizeIs(2));
+  EXPECT_THAT(reduced_types.find("/topic")->second, StrEq("package1/type1"));
 }