diff --git a/rosbag2/CMakeLists.txt b/rosbag2/CMakeLists.txt index e64b3821cf..ca6e9c7e1c 100644 --- a/rosbag2/CMakeLists.txt +++ b/rosbag2/CMakeLists.txt @@ -75,6 +75,14 @@ if(BUILD_TESTING) target_link_libraries(rosbag2_write_integration_test librosbag2) endif() + ament_add_gmock(rosbag2_write_all_integration_test + test/rosbag2/rosbag2_write_all_integration_test.cpp + test/rosbag2/test_memory_management.cpp + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}) + if(TARGET rosbag2_write_all_integration_test) + target_link_libraries(rosbag2_write_all_integration_test librosbag2) + endif() + ament_add_gmock(rosbag2_read_integration_test test/rosbag2/rosbag2_read_integration_test.cpp test/rosbag2/test_memory_management.cpp diff --git a/rosbag2/test/rosbag2/rosbag2_write_all_integration_test.cpp b/rosbag2/test/rosbag2/rosbag2_write_all_integration_test.cpp new file mode 100644 index 0000000000..3c0d19f786 --- /dev/null +++ b/rosbag2/test/rosbag2/rosbag2_write_all_integration_test.cpp @@ -0,0 +1,56 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "rosbag2/rosbag2.hpp" +#include "rosbag2_storage/serialized_bag_message.hpp" +#include "std_msgs/msg/string.hpp" +#include "std_msgs/msg/u_int8.hpp" + +#include "rosbag2_write_integration_fixture.hpp" +#include "test_memory_management.hpp" + +// TODO(Martin-Idel-SI): merge with other write and read tests once signal handling is sorted out +TEST_F(RosBag2IntegrationTestFixture, published_messages_from_multiple_topics_are_recorded) +{ + std::string int_topic = "/int_topic"; + auto serialized_int_bag_message = serialize_message(int_topic, 10); + + std::string string_topic = "/string_topic"; + auto serialized_string_bag_message = serialize_message( + string_topic, "test_message"); + + start_publishing(serialized_string_bag_message, string_topic, 2); + start_publishing(serialized_int_bag_message, int_topic, 2); + + start_recording_all_topics(); + wait_for_publishers(); + stop_recording(); + + auto recorded_messages = get_messages(database_name_); + + ASSERT_THAT(recorded_messages, SizeIs(4)); + auto string_messages = filter_messages(recorded_messages, string_topic); + auto int_messages = filter_messages(recorded_messages, int_topic); + ASSERT_THAT(string_messages, SizeIs(2)); + ASSERT_THAT(int_messages, SizeIs(2)); + EXPECT_THAT(string_messages[0]->data, Eq("test_message")); + EXPECT_THAT(int_messages[0]->data, Eq(10)); +} diff --git a/rosbag2/test/rosbag2/rosbag2_write_integration_fixture.hpp b/rosbag2/test/rosbag2/rosbag2_write_integration_fixture.hpp new file mode 100644 index 0000000000..d662c906c4 --- /dev/null +++ b/rosbag2/test/rosbag2/rosbag2_write_integration_fixture.hpp @@ -0,0 +1,136 @@ +// 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 +#include + +#include "rclcpp/rclcpp.hpp" +#include "rosbag2/rosbag2.hpp" +#include "rosbag2_storage/serialized_bag_message.hpp" + +#include "rosbag2_test_fixture.hpp" +#include "test_memory_management.hpp" + +using namespace ::testing; // NOLINT +using namespace rosbag2; // NOLINT +using namespace std::chrono_literals; // NOLINT + +#ifndef ROSBAG2__ROSBAG2_WRITE_INTEGRATION_FIXTURE_HPP_ +#define ROSBAG2__ROSBAG2_WRITE_INTEGRATION_FIXTURE_HPP_ + +class RosBag2IntegrationTestFixture : public Rosbag2TestFixture +{ +public: + RosBag2IntegrationTestFixture() + : Rosbag2TestFixture() + {} + + void SetUp() override + { + rclcpp::init(0, nullptr); + publisher_node_ = rclcpp::Node::make_shared("publisher_node"); + } + + void record_message(const std::string & db_name, std::vector topics) + { + rosbag2::Rosbag2 rosbag2; + rosbag2.record(db_name, topics, [this](std::string topic_name) { + messages_stored_counter_[topic_name]++; + }); + } + + void start_recording(std::vector topics) + { + // the future object returned from std::async needs to be stored not to block the execution + future_ = std::async( + std::launch::async, [this, topics]() { + record_message(database_name_, topics); + }); + } + + void start_recording_all_topics() + { + // the future object returned from std::async needs to be stored not to block the execution + future_ = std::async( + std::launch::async, [this]() { + record_message(database_name_, {}); + }); + } + + void stop_recording() + { + rclcpp::shutdown(); + future_.get(); + } + + void wait_for_publishers() + { + for (auto & future : publisher_futures_) { + future.get(); + } + } + + void start_publishing( + std::shared_ptr message, + std::string topic_name, size_t number_expected_messages) + { + messages_stored_counter_.insert({topic_name, 0}); + publisher_futures_.push_back(std::async( + std::launch::async, [this, message, topic_name, number_expected_messages]() { + auto publisher = publisher_node_->create_publisher(topic_name); + + while (rclcpp::ok() && messages_stored_counter_[topic_name] < number_expected_messages) { + publisher->publish(message->serialized_data); + // We need to wait a bit, in order for record to process the messages + std::this_thread::sleep_for(50ms); + } + } + )); + } + + template + std::shared_ptr deserialize_message( + std::shared_ptr message) + { + return memory_.deserialize_message(message->serialized_data); + } + + template + std::vector> filter_messages( + std::vector> messages, + std::string topic) + { + std::vector> filtered_messages; + for (const auto & message : messages) { + if (message->topic_name == topic) { + filtered_messages.push_back(deserialize_message(message)); + } + } + return filtered_messages; + } + + rclcpp::Node::SharedPtr publisher_node_; + test_helpers::TestMemoryManagement memory_; + std::map messages_stored_counter_; + std::vector> publisher_futures_; + std::future future_; +}; + +#endif // ROSBAG2__ROSBAG2_WRITE_INTEGRATION_FIXTURE_HPP_ diff --git a/rosbag2/test/rosbag2/rosbag2_write_integration_test.cpp b/rosbag2/test/rosbag2/rosbag2_write_integration_test.cpp index 9ff430fe4e..d0806c692e 100644 --- a/rosbag2/test/rosbag2/rosbag2_write_integration_test.cpp +++ b/rosbag2/test/rosbag2/rosbag2_write_integration_test.cpp @@ -14,12 +14,9 @@ #include -#include -#include #include #include #include -#include #include "rclcpp/rclcpp.hpp" #include "rosbag2/rosbag2.hpp" @@ -27,103 +24,10 @@ #include "std_msgs/msg/string.hpp" #include "std_msgs/msg/u_int8.hpp" -#include "rosbag2_test_fixture.hpp" +#include "rosbag2_write_integration_fixture.hpp" #include "test_memory_management.hpp" -using namespace ::testing; // NOLINT -using namespace rosbag2; // NOLINT -using namespace std::chrono_literals; // NOLINT - // TODO(Martin-Idel-SI): merge with other write and read tests once signal handling is sorted out -class RosBag2IntegrationTestFixture : public Rosbag2TestFixture -{ -public: - RosBag2IntegrationTestFixture() - : Rosbag2TestFixture() - {} - - void SetUp() override - { - rclcpp::init(0, nullptr); - publisher_node_ = rclcpp::Node::make_shared("publisher_node"); - } - - void record_message(const std::string & db_name, std::vector topics) - { - rosbag2::Rosbag2 rosbag2; - rosbag2.record(db_name, topics, [this](std::string topic_name) { - messages_stored_counter_[topic_name]++; - }); - } - - void start_recording(std::vector topics) - { - // the future object returned from std::async needs to be stored not to block the execution - future_ = std::async( - std::launch::async, [this, topics]() { - record_message(database_name_, topics); - }); - } - - void stop_recording() - { - rclcpp::shutdown(); - future_.get(); - } - - void wait_for_publishers() - { - for (auto & future : publisher_futures_) { - future.get(); - } - } - - void start_publishing( - std::shared_ptr message, - std::string topic_name, size_t number_expected_messages) - { - messages_stored_counter_.insert({topic_name, 0}); - publisher_futures_.push_back(std::async( - std::launch::async, [this, message, topic_name, number_expected_messages]() { - auto publisher = publisher_node_->create_publisher(topic_name); - - while (rclcpp::ok() && messages_stored_counter_[topic_name] < number_expected_messages) { - publisher->publish(message->serialized_data); - // We need to wait a bit, in order for record to process the messages - std::this_thread::sleep_for(50ms); - } - } - )); - } - - template - std::shared_ptr deserialize_message( - std::shared_ptr message) - { - return memory_.deserialize_message(message->serialized_data); - } - - template - std::vector> filter_messages( - std::vector> messages, - std::string topic) - { - std::vector> filtered_messages; - for (const auto & message : messages) { - if (message->topic_name == topic) { - filtered_messages.push_back(deserialize_message(message)); - } - } - return filtered_messages; - } - - rclcpp::Node::SharedPtr publisher_node_; - test_helpers::TestMemoryManagement memory_; - std::map messages_stored_counter_; - std::vector> publisher_futures_; - std::future future_; -}; - TEST_F(RosBag2IntegrationTestFixture, published_messages_from_multiple_topics_are_recorded) { std::string int_topic = "/int_topic";