diff --git a/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp b/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp index 0f94873cf0..1695203185 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp @@ -117,6 +117,7 @@ class ROSBAG2_CPP_PUBLIC SequentialWriter /** * Write a message to a bagfile. The topic needs to have been created before writing is possible. + * Only writes message if within start_time_ns and end_time_ns (from storage_options). * * \param message to be written to the bagfile * \throws runtime_error if the Writer is not open. @@ -174,6 +175,10 @@ class ROSBAG2_CPP_PUBLIC SequentialWriter bool should_split_bagfile( const std::chrono::time_point & current_time) const; + // Checks if the message to be written is within accepted time range + bool message_within_accepted_time_range( + const rcutils_time_point_value_t current_time) const; + // Prepares the metadata by setting initial values. virtual void init_metadata(); diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index d32951e6f7..2bbafa689b 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -340,6 +340,10 @@ void SequentialWriter::write(std::shared_ptrtime_stamp)) { + return; + } + // Get TopicInformation handler for counting messages. rosbag2_storage::TopicInformation * topic_information {nullptr}; try { @@ -432,6 +436,24 @@ bool SequentialWriter::should_split_bagfile( return should_split; } +bool SequentialWriter::message_within_accepted_time_range( + const rcutils_time_point_value_t current_time) const +{ + if (storage_options_.start_time_ns >= 0 && + static_cast(current_time) < storage_options_.start_time_ns) + { + return false; + } + + if (storage_options_.end_time_ns >= 0 && + static_cast(current_time) > storage_options_.end_time_ns) + { + return false; + } + + return true; +} + void SequentialWriter::finalize_metadata() { metadata_.bag_size = 0; diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index 4ecdcb2c4f..56a719bb38 100644 --- a/rosbag2_py/src/rosbag2_py/_storage.cpp +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -84,7 +84,7 @@ PYBIND11_MODULE(_storage, m) { .def( pybind11::init< std::string, std::string, uint64_t, uint64_t, uint64_t, std::string, std::string, bool, - KEY_VALUE_MAP>(), + int64_t, int64_t, KEY_VALUE_MAP>(), pybind11::arg("uri"), pybind11::arg("storage_id") = "", pybind11::arg("max_bagfile_size") = 0, @@ -93,6 +93,8 @@ PYBIND11_MODULE(_storage, m) { pybind11::arg("storage_preset_profile") = "", pybind11::arg("storage_config_uri") = "", pybind11::arg("snapshot_mode") = false, + pybind11::arg("start_time_ns") = -1, + pybind11::arg("end_time_ns") = -1, pybind11::arg("custom_data") = KEY_VALUE_MAP{}) .def_readwrite("uri", &rosbag2_storage::StorageOptions::uri) .def_readwrite("storage_id", &rosbag2_storage::StorageOptions::storage_id) @@ -114,6 +116,12 @@ PYBIND11_MODULE(_storage, m) { .def_readwrite( "snapshot_mode", &rosbag2_storage::StorageOptions::snapshot_mode) + .def_readwrite( + "start_time_ns", + &rosbag2_storage::StorageOptions::start_time_ns) + .def_readwrite( + "end_time_ns", + &rosbag2_storage::StorageOptions::end_time_ns) .def_readwrite( "custom_data", &rosbag2_storage::StorageOptions::custom_data); diff --git a/rosbag2_storage/include/rosbag2_storage/storage_options.hpp b/rosbag2_storage/include/rosbag2_storage/storage_options.hpp index 5dd5be67fb..8d6d188459 100644 --- a/rosbag2_storage/include/rosbag2_storage/storage_options.hpp +++ b/rosbag2_storage/include/rosbag2_storage/storage_options.hpp @@ -56,6 +56,10 @@ struct StorageOptions // Defaults to disabled. bool snapshot_mode = false; + // Start and end time for cutting + int64_t start_time_ns = -1; + int64_t end_time_ns = -1; + // Stores the custom data std::unordered_map custom_data{}; }; diff --git a/rosbag2_storage/src/rosbag2_storage/storage_options.cpp b/rosbag2_storage/src/rosbag2_storage/storage_options.cpp index b6925666d4..717fcb9082 100644 --- a/rosbag2_storage/src/rosbag2_storage/storage_options.cpp +++ b/rosbag2_storage/src/rosbag2_storage/storage_options.cpp @@ -32,6 +32,8 @@ Node convert::encode( node["storage_preset_profile"] = storage_options.storage_preset_profile; node["storage_config_uri"] = storage_options.storage_config_uri; node["snapshot_mode"] = storage_options.snapshot_mode; + node["start_time_ns"] = storage_options.start_time_ns; + node["end_time_ns"] = storage_options.end_time_ns; node["custom_data"] = storage_options.custom_data; return node; } @@ -48,6 +50,8 @@ bool convert::decode( node, "storage_preset_profile", storage_options.storage_preset_profile); optional_assign(node, "storage_config_uri", storage_options.storage_config_uri); optional_assign(node, "snapshot_mode", storage_options.snapshot_mode); + optional_assign(node, "start_time_ns", storage_options.start_time_ns); + optional_assign(node, "end_time_ns", storage_options.end_time_ns); using KEY_VALUE_MAP = std::unordered_map; optional_assign(node, "custom_data", storage_options.custom_data); return true; diff --git a/rosbag2_storage/test/rosbag2_storage/test_storage_options.cpp b/rosbag2_storage/test/rosbag2_storage/test_storage_options.cpp index 3163201ac5..aa21761178 100644 --- a/rosbag2_storage/test/rosbag2_storage/test_storage_options.cpp +++ b/rosbag2_storage/test/rosbag2_storage/test_storage_options.cpp @@ -29,6 +29,8 @@ TEST(storage_options, test_yaml_serialization) original.storage_preset_profile = "profile"; original.storage_config_uri = "config_uri"; original.snapshot_mode = true; + original.start_time_ns = 12345000; + original.end_time_ns = 23456000; original.custom_data["key1"] = "value1"; original.custom_data["key2"] = "value2"; @@ -48,5 +50,7 @@ TEST(storage_options, test_yaml_serialization) ASSERT_EQ(original.storage_preset_profile, reconstructed.storage_preset_profile); ASSERT_EQ(original.storage_config_uri, reconstructed.storage_config_uri); ASSERT_EQ(original.snapshot_mode, reconstructed.snapshot_mode); + ASSERT_EQ(original.start_time_ns, reconstructed.start_time_ns); + ASSERT_EQ(original.end_time_ns, reconstructed.end_time_ns); ASSERT_EQ(original.custom_data, reconstructed.custom_data); }