diff --git a/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp b/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp index adb91d1f31..0b2ed5338b 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp @@ -121,6 +121,11 @@ class ROSBAG2_CPP_PUBLIC SequentialWriter */ void add_event_callbacks(const bag_events::WriterEventCallbacks & callbacks) override; + /** + * \brief Closes the current backed storage and opens the next bagfile. + */ + void split_bagfile() override; + protected: std::string base_folder_; std::unique_ptr storage_factory_; @@ -146,9 +151,6 @@ class ROSBAG2_CPP_PUBLIC SequentialWriter rosbag2_storage::BagMetadata metadata_; - // Closes the current backed storage and opens the next bagfile. - void split_bagfile() override; - // Checks if the current recording bagfile needs to be split and rolled over to a new file. bool should_split_bagfile( const std::chrono::time_point & current_time) const; diff --git a/rosbag2_interfaces/srv/SplitBagfile.srv b/rosbag2_interfaces/srv/SplitBagfile.srv index 73b314ff7c..ed97d539c0 100644 --- a/rosbag2_interfaces/srv/SplitBagfile.srv +++ b/rosbag2_interfaces/srv/SplitBagfile.srv @@ -1 +1 @@ ---- \ No newline at end of file +--- diff --git a/rosbag2_transport/src/rosbag2_transport/recorder.cpp b/rosbag2_transport/src/rosbag2_transport/recorder.cpp index 22a8ece578..72fe3aa594 100644 --- a/rosbag2_transport/src/rosbag2_transport/recorder.cpp +++ b/rosbag2_transport/src/rosbag2_transport/recorder.cpp @@ -153,8 +153,8 @@ void Recorder::record() "~/split_bagfile", [this]( const std::shared_ptr/* request_header */, - const std::shared_ptr, - const std::shared_ptr response) + const std::shared_ptr/* request */, + const std::shared_ptr/* response */) { writer_->split_bagfile(); }); diff --git a/rosbag2_transport/test/rosbag2_transport/mock_sequential_writer.hpp b/rosbag2_transport/test/rosbag2_transport/mock_sequential_writer.hpp index 58075e3453..c8a55c2f19 100644 --- a/rosbag2_transport/test/rosbag2_transport/mock_sequential_writer.hpp +++ b/rosbag2_transport/test/rosbag2_transport/mock_sequential_writer.hpp @@ -76,7 +76,17 @@ class MockSequentialWriter : public rosbag2_cpp::writer_interfaces::BaseWriterIn void split_bagfile() override { auto info = std::make_shared(); + info->closed_file = "BagFile" + std::to_string(file_number_); + file_number_ += 1; + info->opened_file = "BagFile" + std::to_string(file_number_); callback_manager_.execute_callbacks(rosbag2_cpp::bag_events::BagEvent::WRITE_SPLIT, info); + messages_per_file_ = 0; + split_bagfile_called_ = true; + } + + bool split_bagfile_called() + { + return split_bagfile_called_; } void @@ -125,6 +135,7 @@ class MockSequentialWriter : public rosbag2_cpp::writer_interfaces::BaseWriterIn rosbag2_cpp::bag_events::EventCallbackManager callback_manager_; size_t file_number_ = 0; const size_t max_messages_per_file_ = 5; + bool split_bagfile_called_ = false; }; #endif // ROSBAG2_TRANSPORT__MOCK_SEQUENTIAL_WRITER_HPP_ diff --git a/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp b/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp index 7baa6ee4dc..d69df04e84 100644 --- a/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp +++ b/rosbag2_transport/test/rosbag2_transport/test_record_services.cpp @@ -22,6 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "rosbag2_interfaces/srv/snapshot.hpp" +#include "rosbag2_interfaces/srv/split_bagfile.hpp" #include "rosbag2_transport/recorder.hpp" #include "rosbag2_test_common/publication_manager.hpp" @@ -40,9 +41,11 @@ class RecordSrvsTest : public RecordIntegrationTestFixture { public: using Snapshot = rosbag2_interfaces::srv::Snapshot; + using SplitBagfile = rosbag2_interfaces::srv::SplitBagfile; - RecordSrvsTest() - : RecordIntegrationTestFixture() + explicit RecordSrvsTest(const bool snapshot_mode) + : RecordIntegrationTestFixture(), + snapshot_mode_(snapshot_mode) {} ~RecordSrvsTest() override @@ -68,7 +71,7 @@ class RecordSrvsTest : public RecordIntegrationTestFixture rosbag2_transport::RecordOptions record_options = {false, false, {test_topic_}, "rmw_format", 100ms}; - storage_options_.snapshot_mode = true; + storage_options_.snapshot_mode = snapshot_mode_; storage_options_.max_cache_size = 200; recorder_ = std::make_shared( std::move(writer_), storage_options_, record_options, recorder_name_); @@ -80,6 +83,7 @@ class RecordSrvsTest : public RecordIntegrationTestFixture const std::string ns = "/" + recorder_name_; cli_snapshot_ = client_node_->create_client(ns + "/snapshot"); + cli_split_bagfile_ = client_node_->create_client(ns + "/split_bagfile"); exec_ = std::make_shared(); @@ -94,7 +98,10 @@ class RecordSrvsTest : public RecordIntegrationTestFixture pub_manager.run_publishers(); // Make sure expected service is present before starting test - ASSERT_TRUE(cli_snapshot_->wait_for_service(service_wait_timeout_)); + if (snapshot_mode_) { + ASSERT_TRUE(cli_snapshot_->wait_for_service(service_wait_timeout_)); + } + ASSERT_TRUE(cli_split_bagfile_->wait_for_service(service_wait_timeout_)); } /// Send a service request, and expect it to successfully return within a reasonable timeout @@ -135,9 +142,19 @@ class RecordSrvsTest : public RecordIntegrationTestFixture // Service clients rclcpp::Node::SharedPtr client_node_; rclcpp::Client::SharedPtr cli_snapshot_; + rclcpp::Client::SharedPtr cli_split_bagfile_; + + bool snapshot_mode_; }; -TEST_F(RecordSrvsTest, trigger_snapshot) +class RecordSrvsSnapshotTest : public RecordSrvsTest +{ +protected: + RecordSrvsSnapshotTest() + : RecordSrvsTest(true /*snapshot_mode*/) {} +}; + +TEST_F(RecordSrvsSnapshotTest, trigger_snapshot) { auto & writer = recorder_->get_writer_handle(); MockSequentialWriter & mock_writer = @@ -152,3 +169,25 @@ TEST_F(RecordSrvsTest, trigger_snapshot) successful_service_request(cli_snapshot_); EXPECT_THAT(mock_writer.get_messages().size(), Ne(0u)); } + +class RecordSrvsSplitBagfileTest : public RecordSrvsTest +{ +protected: + RecordSrvsSplitBagfileTest() + : RecordSrvsTest(false /*snapshot_mode*/) {} +}; + +TEST_F(RecordSrvsSplitBagfileTest, split_bagfile) +{ + auto & writer = recorder_->get_writer_handle(); + MockSequentialWriter & mock_writer = + static_cast(writer.get_implementation_handle()); + EXPECT_FALSE(mock_writer.split_bagfile_called()); + + std::chrono::duration duration(2.0); + std::this_thread::sleep_for(duration); + EXPECT_FALSE(mock_writer.split_bagfile_called()); + + successful_service_request(cli_split_bagfile_); + EXPECT_TRUE(mock_writer.split_bagfile_called()); +}