Skip to content

Commit

Permalink
feat(rosbag2_cpp): Add unit tests for SplitBagfile feature.
Browse files Browse the repository at this point in the history
Also address PR comments from @MichaelOrlov and deal with rebase merge
conflicts.

Signed-off-by: Rick Shanor <rickshanor@gmail.com>
  • Loading branch information
rshanor committed Oct 6, 2022
1 parent 6a23f80 commit f382130
Show file tree
Hide file tree
Showing 5 changed files with 63 additions and 11 deletions.
8 changes: 5 additions & 3 deletions rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<rosbag2_storage::StorageFactoryInterface> storage_factory_;
Expand All @@ -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<std::chrono::high_resolution_clock> & current_time) const;
Expand Down
2 changes: 1 addition & 1 deletion rosbag2_interfaces/srv/SplitBagfile.srv
Original file line number Diff line number Diff line change
@@ -1 +1 @@
---
---
4 changes: 2 additions & 2 deletions rosbag2_transport/src/rosbag2_transport/recorder.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -153,8 +153,8 @@ void Recorder::record()
"~/split_bagfile",
[this](
const std::shared_ptr<rmw_request_id_t>/* request_header */,
const std::shared_ptr<rosbag2_interfaces::srv::SplitBagfile::Request>,
const std::shared_ptr<rosbag2_interfaces::srv::SplitBagfile::Response> response)
const std::shared_ptr<rosbag2_interfaces::srv::SplitBagfile::Request>/* request */,
const std::shared_ptr<rosbag2_interfaces::srv::SplitBagfile::Response>/* response */)
{
writer_->split_bagfile();
});
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,17 @@ class MockSequentialWriter : public rosbag2_cpp::writer_interfaces::BaseWriterIn
void split_bagfile() override
{
auto info = std::make_shared<rosbag2_cpp::bag_events::BagSplitInfo>();
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
Expand Down Expand Up @@ -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_
49 changes: 44 additions & 5 deletions rosbag2_transport/test/rosbag2_transport/test_record_services.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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
Expand All @@ -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<rosbag2_transport::Recorder>(
std::move(writer_), storage_options_, record_options, recorder_name_);
Expand All @@ -80,6 +83,7 @@ class RecordSrvsTest : public RecordIntegrationTestFixture

const std::string ns = "/" + recorder_name_;
cli_snapshot_ = client_node_->create_client<Snapshot>(ns + "/snapshot");
cli_split_bagfile_ = client_node_->create_client<SplitBagfile>(ns + "/split_bagfile");

exec_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();

Expand All @@ -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
Expand Down Expand Up @@ -135,9 +142,19 @@ class RecordSrvsTest : public RecordIntegrationTestFixture
// Service clients
rclcpp::Node::SharedPtr client_node_;
rclcpp::Client<Snapshot>::SharedPtr cli_snapshot_;
rclcpp::Client<SplitBagfile>::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 =
Expand All @@ -152,3 +169,25 @@ TEST_F(RecordSrvsTest, trigger_snapshot)
successful_service_request<Snapshot>(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<MockSequentialWriter &>(writer.get_implementation_handle());
EXPECT_FALSE(mock_writer.split_bagfile_called());

std::chrono::duration<float> duration(2.0);
std::this_thread::sleep_for(duration);
EXPECT_FALSE(mock_writer.split_bagfile_called());

successful_service_request<SplitBagfile>(cli_split_bagfile_);
EXPECT_TRUE(mock_writer.split_bagfile_called());
}

0 comments on commit f382130

Please sign in to comment.