From 235116168c0fcc1068da891a6e1d336a9faee541 Mon Sep 17 00:00:00 2001 From: Martin Idel Date: Wed, 29 Aug 2018 12:21:22 +0200 Subject: [PATCH] GH-23 Move methods to node for better interface --- rosbag2/CMakeLists.txt | 8 -- rosbag2/include/rosbag2/rosbag2.hpp | 12 -- rosbag2/src/rosbag2/rosbag2.cpp | 69 +--------- rosbag2/src/rosbag2/rosbag2_node.cpp | 66 +++++++++ rosbag2/src/rosbag2/rosbag2_node.hpp | 10 ++ .../test/rosbag2/rosbag2_integration_test.cpp | 129 ------------------ .../test/rosbag2/rosbag2_rosbag_node_test.cpp | 77 +++++++++++ 7 files changed, 155 insertions(+), 216 deletions(-) delete mode 100644 rosbag2/test/rosbag2/rosbag2_integration_test.cpp diff --git a/rosbag2/CMakeLists.txt b/rosbag2/CMakeLists.txt index b8fcd949ea..df0e18a181 100644 --- a/rosbag2/CMakeLists.txt +++ b/rosbag2/CMakeLists.txt @@ -110,14 +110,6 @@ if(BUILD_TESTING) ) endif() - ament_add_gmock(rosbag2_integration_test - test/rosbag2/rosbag2_integration_test.cpp - src/rosbag2/typesupport_helpers.cpp - WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) - if(TARGET rosbag2_integration_test) - target_link_libraries(rosbag2_integration_test librosbag2) - endif() - ament_add_gmock(rosbag2_rosbag_node_test test/rosbag2/rosbag2_rosbag_node_test.cpp test/rosbag2/test_memory_management.cpp diff --git a/rosbag2/include/rosbag2/rosbag2.hpp b/rosbag2/include/rosbag2/rosbag2.hpp index 5b52c9d64c..20d9c66486 100644 --- a/rosbag2/include/rosbag2/rosbag2.hpp +++ b/rosbag2/include/rosbag2/rosbag2.hpp @@ -62,15 +62,6 @@ class Rosbag2 ROSBAG2_PUBLIC void play(const std::string & file_name); - /// Exposed for testing - ROSBAG2_PUBLIC - std::map get_topics_with_types( - const std::vector & topic_names, std::shared_ptr node); - - /// Exposed for testing - ROSBAG2_PUBLIC - std::map get_all_topics_with_types(std::shared_ptr node); - private: void prepare_publishers( std::shared_ptr node, @@ -83,9 +74,6 @@ class Rosbag2 std::shared_ptr & node, const std::string & topic_name, const std::string & topic_type) const; - std::map sanitize_topics_and_types( - std::map> topics_and_types); - std::vector> subscriptions_; std::map> publishers_; }; diff --git a/rosbag2/src/rosbag2/rosbag2.cpp b/rosbag2/src/rosbag2/rosbag2.cpp index de5b922a22..883f7f21e2 100644 --- a/rosbag2/src/rosbag2/rosbag2.cpp +++ b/rosbag2/src/rosbag2/rosbag2.cpp @@ -14,12 +14,10 @@ #include "rosbag2/rosbag2.hpp" -#include #include #include #include #include -#include #include #include "rcl/graph.h" @@ -58,8 +56,8 @@ void Rosbag2::record( auto node = std::make_shared("rosbag2"); auto topics_and_types = topic_names.empty() ? - get_all_topics_with_types(node) : - get_topics_with_types(topic_names, node); + node->get_all_topics_with_types() : + node->get_topics_with_types(topic_names); if (topics_and_types.empty()) { throw std::runtime_error("No topics found. Abort"); @@ -152,67 +150,4 @@ void Rosbag2::prepare_publishers( } } -std::map Rosbag2::get_topics_with_types( - const std::vector & topic_names, std::shared_ptr node) -{ - 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; - }); - - // 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> 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 -Rosbag2::get_all_topics_with_types(std::shared_ptr 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 Rosbag2::sanitize_topics_and_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) { - 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 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 diff --git a/rosbag2/src/rosbag2/rosbag2_node.cpp b/rosbag2/src/rosbag2/rosbag2_node.cpp index d87e8b3eb0..14bbe3821e 100644 --- a/rosbag2/src/rosbag2/rosbag2_node.cpp +++ b/rosbag2/src/rosbag2/rosbag2_node.cpp @@ -14,8 +14,12 @@ #include "rosbag2_node.hpp" +#include +#include #include #include +#include +#include #include "typesupport_helpers.hpp" @@ -57,5 +61,67 @@ std::shared_ptr Rosbag2Node::create_generic_subscription( return subscription; } +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; + }); + + // 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> 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 +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()); +} + +std::map Rosbag2Node::sanitize_topics_and_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) { + RCUTILS_LOG_ERROR_NAMED( + "rosbag2", + "Topic '%s' has several types associated. Only topics with one type are supported.", + element.first.c_str()); + return true; + } + return false; + }); + + 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]); + }); + return topics_and_types_to_record; +} } // namespace rosbag2 diff --git a/rosbag2/src/rosbag2/rosbag2_node.hpp b/rosbag2/src/rosbag2/rosbag2_node.hpp index f25c71f13b..1e17518504 100644 --- a/rosbag2/src/rosbag2/rosbag2_node.hpp +++ b/rosbag2/src/rosbag2/rosbag2_node.hpp @@ -15,8 +15,10 @@ #ifndef ROSBAG2__ROSBAG2_NODE_HPP_ #define ROSBAG2__ROSBAG2_NODE_HPP_ +#include #include #include +#include #include "rclcpp/node.hpp" #include "rcutils/types.h" @@ -40,6 +42,14 @@ class Rosbag2Node : public rclcpp::Node const std::string & topic, const std::string & type, std::function)> callback); + + std::map + get_topics_with_types(const std::vector & topic_names); + + std::map get_all_topics_with_types(); + + std::map sanitize_topics_and_types( + std::map> topics_and_types); }; } // namespace rosbag2 diff --git a/rosbag2/test/rosbag2/rosbag2_integration_test.cpp b/rosbag2/test/rosbag2/rosbag2_integration_test.cpp deleted file mode 100644 index e354a4717b..0000000000 --- a/rosbag2/test/rosbag2/rosbag2_integration_test.cpp +++ /dev/null @@ -1,129 +0,0 @@ -// Copyright 2018, Bosch Software Innovations GmbH. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "rosbag2/rosbag2.hpp" -#include "std_msgs/msg/string.hpp" - -#include "rosbag2_test_fixture.hpp" - -using namespace ::testing; // NOLINT - -class RosBag2IntegrationFixture : public Test -{ -public: - RosBag2IntegrationFixture() - { - node_ = std::make_shared("rosbag2"); - publisher_node_ = std::make_shared("publisher_node"); - rosbag2_ = rosbag2::Rosbag2(); - } - - void create_publisher(std::string topic) - { - auto publisher = publisher_node_->create_publisher(topic); - publishers_.push_back(publisher); - } - - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - - rosbag2::Rosbag2 rosbag2_; - std::shared_ptr node_; - std::shared_ptr publisher_node_; - std::vector> 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_topics_with_types_returns_with_topic_string_if_topic_is_specified_without_slash) -{ - create_publisher("string_topic"); - - auto topics_and_types = rosbag2_.get_topics_with_types({"string_topic"}, node_); - - ASSERT_THAT(topics_and_types, SizeIs(1)); - EXPECT_THAT(topics_and_types.begin()->second, StrEq("std_msgs/String")); -} - -TEST_F(RosBag2IntegrationFixture, - get_topics_with_types_returns_with_topic_string_if_topic_is_specified_with_slash) -{ - create_publisher("string_topic"); - - auto topics_and_types = rosbag2_.get_topics_with_types({"/string_topic"}, node_); - - ASSERT_THAT(topics_and_types, SizeIs(1)); - EXPECT_THAT(topics_and_types.begin()->second, StrEq("std_msgs/String")); -} - -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"); - std::string third_topic("/wrong_topic"); - - create_publisher(first_topic); - create_publisher(second_topic); - create_publisher(third_topic); - - auto topics_and_types = rosbag2_.get_topics_with_types({first_topic, second_topic}, node_); - - ASSERT_THAT(topics_and_types, SizeIs(2)); - 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")); -} - -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"); - - create_publisher(first_topic); - create_publisher(second_topic); - create_publisher(third_topic); - - 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")); -} diff --git a/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp b/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp index 4adc67f191..7cb873a16d 100644 --- a/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp +++ b/rosbag2/test/rosbag2/rosbag2_rosbag_node_test.cpp @@ -34,6 +34,7 @@ class RosBag2NodeFixture : public Test RosBag2NodeFixture() { node_ = std::make_shared("rosbag2"); + publisher_node_ = std::make_shared("publisher_node"); } static void SetUpTestCase() @@ -46,6 +47,12 @@ class RosBag2NodeFixture : public Test rclcpp::shutdown(); } + void create_publisher(std::string topic) + { + auto publisher = publisher_node_->create_publisher(topic); + publishers_.push_back(publisher); + } + std::vector subscribe_raw_messages( size_t expected_messages_number, const std::string & topic_name, const std::string & type) { @@ -74,6 +81,8 @@ class RosBag2NodeFixture : public Test test_helpers::TestMemoryManagement memory_management_; std::shared_ptr node_; + rclcpp::Node::SharedPtr publisher_node_; + std::vector> publishers_; }; @@ -100,3 +109,71 @@ TEST_F(RosBag2NodeFixture, publisher_and_subscriber_work) EXPECT_THAT(subscribed_messages, SizeIs(Not(0))); EXPECT_THAT(subscribed_messages[0], StrEq("Hello World")); } + +TEST_F(RosBag2NodeFixture, get_topics_with_types_returns_empty_if_topic_does_not_exist) { + create_publisher("string_topic"); + + auto topics_and_types = node_->get_topics_with_types({"/wrong_topic"}); + + ASSERT_THAT(topics_and_types, IsEmpty()); +} + +TEST_F(RosBag2NodeFixture, + get_topics_with_types_returns_with_topic_string_if_topic_is_specified_without_slash) +{ + create_publisher("string_topic"); + + auto topics_and_types = node_->get_topics_with_types({"string_topic"}); + + ASSERT_THAT(topics_and_types, SizeIs(1)); + EXPECT_THAT(topics_and_types.begin()->second, StrEq("std_msgs/String")); +} + +TEST_F(RosBag2NodeFixture, + get_topics_with_types_returns_with_topic_string_if_topic_is_specified_with_slash) +{ + create_publisher("string_topic"); + + auto topics_and_types = node_->get_topics_with_types({"/string_topic"}); + + ASSERT_THAT(topics_and_types, SizeIs(1)); + EXPECT_THAT(topics_and_types.begin()->second, StrEq("std_msgs/String")); +} + +TEST_F(RosBag2NodeFixture, get_topics_with_types_returns_multiple_topics_for_multiple_inputs) { + std::string first_topic("/string_topic"); + std::string second_topic("/other_topic"); + std::string third_topic("/wrong_topic"); + + create_publisher(first_topic); + create_publisher(second_topic); + create_publisher(third_topic); + + auto topics_and_types = node_->get_topics_with_types({first_topic, second_topic}); + + ASSERT_THAT(topics_and_types, SizeIs(2)); + 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")); +} + +TEST_F(RosBag2NodeFixture, 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"); + + create_publisher(first_topic); + create_publisher(second_topic); + create_publisher(third_topic); + + auto topics_and_types = node_->get_all_topics_with_types(); + + 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")); +}