From 9ab0ee2fde5ddcad76d1aace104788ccb2cb57ca Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Thu, 30 Aug 2018 14:17:07 +0200 Subject: [PATCH] GH-23 Improve readability of sanitizing topics and types --- rosbag2/src/rosbag2/rosbag2_node.cpp | 88 +++++++++---------- rosbag2/src/rosbag2/rosbag2_node.hpp | 5 +- .../test/rosbag2/rosbag2_rosbag_node_test.cpp | 29 +++--- 3 files changed, 67 insertions(+), 55 deletions(-) diff --git a/rosbag2/src/rosbag2/rosbag2_node.cpp b/rosbag2/src/rosbag2/rosbag2_node.cpp index 2f2233b8df..4da1107d66 100644 --- a/rosbag2/src/rosbag2/rosbag2_node.cpp +++ b/rosbag2/src/rosbag2/rosbag2_node.cpp @@ -65,10 +65,9 @@ std::map Rosbag2Node::get_topics_with_types( const std::vector & topic_names) { std::vector 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 @@ -76,14 +75,15 @@ std::map Rosbag2Node::get_topics_with_types( auto topics_and_types = this->get_topic_names_and_types(); std::map> 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 @@ -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 Rosbag2Node::sanitize_topics_and_types( +std::map> Rosbag2Node::filter_topics_with_wrong_types( std::map> topics_and_types) { std::map> 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 Rosbag2Node::reduce_multiple_types_to_one( + std::map> topics_and_types) +{ std::map 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 index 162d053e90..6d0aaf0149 100644 --- 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 get_all_topics_with_types(); - std::map sanitize_topics_and_types( + std::map> filter_topics_with_wrong_types( + std::map> topics_and_types); + + std::map reduce_multiple_types_to_one( std::map> topics_and_types); }; diff --git a/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp b/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp index 9146e9229f..03f8066f45 100644 --- a/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp +++ b/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp @@ -179,20 +179,18 @@ 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> 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> topics_and_types = { {"/topic", {"package1/type1"}}, {"/topic2", {"package1/"}}, @@ -200,8 +198,19 @@ TEST_F(RosBag2NodeFixture, sanitize_topics_and_types_deletes_topics_with_wrong_t {"/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> 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")); }