From a11905f11b93dff5e76ba9ac2fb1a7b6fbc542b4 Mon Sep 17 00:00:00 2001 From: Peter Favrholdt Date: Sun, 3 Sep 2023 16:37:17 +0200 Subject: [PATCH 01/13] ros2 bag convert now excludes messages not in [start_time;end_time] resolves https://github.com/ros2/rosbag2/issues/599 Signed-off-by: Peter Favrholdt --- .../rosbag2_cpp/writers/sequential_writer.hpp | 4 ++++ .../src/rosbag2_cpp/writers/sequential_writer.cpp | 14 ++++++++++++++ rosbag2_py/src/rosbag2_py/_storage.cpp | 10 +++++++++- rosbag2_storage/CMakeLists.txt | 2 ++ .../include/rosbag2_storage/storage_options.hpp | 5 +++++ .../src/rosbag2_storage/storage_options.cpp | 4 ++++ rosbag2_storage_mcap/CMakeLists.txt | 2 ++ rosbag2_storage_mcap/src/mcap_storage.cpp | 2 +- rosbag2_storage_sqlite3/CMakeLists.txt | 2 ++ 9 files changed, 43 insertions(+), 2 deletions(-) diff --git a/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp b/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp index 0f94873cf0..cd5e27cf69 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp @@ -174,6 +174,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 std::chrono::time_point & 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..86d41e4bdf 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -354,6 +354,8 @@ void SequentialWriter::write(std::shared_ptr( std::chrono::nanoseconds(message->time_stamp)); + if (! message_within_accepted_time_range(message_timestamp)) return; + if (is_first_message_) { // Update bagfile starting time metadata_.starting_time = message_timestamp; @@ -432,6 +434,18 @@ bool SequentialWriter::should_split_bagfile( return should_split; } +bool SequentialWriter::message_within_accepted_time_range(const std::chrono::time_point ¤t_time) const +{ + bool retVal=true; + if ( (storage_options_.start_time > 0) && + current_time < std::chrono::time_point(std::chrono::nanoseconds(storage_options_.start_time)) ) + retVal = false; + else if ( ( storage_options_.end_time > 0) && + current_time > std::chrono::time_point(std::chrono::nanoseconds(storage_options_.end_time)) ) + retVal = false; + return retVal; +} + 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..509581bf29 100644 --- a/rosbag2_py/src/rosbag2_py/_storage.cpp +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -83,7 +83,7 @@ PYBIND11_MODULE(_storage, m) { pybind11::class_(m, "StorageOptions") .def( pybind11::init< - std::string, std::string, uint64_t, uint64_t, uint64_t, std::string, std::string, bool, + std::string, std::string, uint64_t, uint64_t, uint64_t, std::string, std::string, bool, rcutils_time_point_value_t, rcutils_time_point_value_t, KEY_VALUE_MAP>(), pybind11::arg("uri"), pybind11::arg("storage_id") = "", @@ -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") = 0, + pybind11::arg("end_time") = 0, 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", + &rosbag2_storage::StorageOptions::start_time) + .def_readwrite( + "end_time", + &rosbag2_storage::StorageOptions::end_time) .def_readwrite( "custom_data", &rosbag2_storage::StorageOptions::custom_data); diff --git a/rosbag2_storage/CMakeLists.txt b/rosbag2_storage/CMakeLists.txt index cd4d7ab601..3d61538095 100644 --- a/rosbag2_storage/CMakeLists.txt +++ b/rosbag2_storage/CMakeLists.txt @@ -22,6 +22,7 @@ if(WIN32) endif() find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) find_package(pluginlib REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) @@ -44,6 +45,7 @@ target_include_directories(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} pluginlib::pluginlib rcpputils::rcpputils + rclcpp::rclcpp rcutils::rcutils yaml-cpp ) diff --git a/rosbag2_storage/include/rosbag2_storage/storage_options.hpp b/rosbag2_storage/include/rosbag2_storage/storage_options.hpp index 5dd5be67fb..c1d8387f17 100644 --- a/rosbag2_storage/include/rosbag2_storage/storage_options.hpp +++ b/rosbag2_storage/include/rosbag2_storage/storage_options.hpp @@ -21,6 +21,7 @@ #include "rosbag2_storage/visibility_control.hpp" #include "rosbag2_storage/yaml.hpp" +#include "rclcpp/time.hpp" namespace rosbag2_storage { @@ -56,6 +57,10 @@ struct StorageOptions // Defaults to disabled. bool snapshot_mode = false; + // Start and end time for cutting + rcutils_time_point_value_t start_time = 0; + rcutils_time_point_value_t end_time = 0; + // 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..1f2cabd09d 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"] = storage_options.start_time; + node["end_time"] = storage_options.end_time; 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", storage_options.start_time); + optional_assign(node, "end_time", storage_options.end_time); using KEY_VALUE_MAP = std::unordered_map; optional_assign(node, "custom_data", storage_options.custom_data); return true; diff --git a/rosbag2_storage_mcap/CMakeLists.txt b/rosbag2_storage_mcap/CMakeLists.txt index f19e9f3446..441ca44ad3 100644 --- a/rosbag2_storage_mcap/CMakeLists.txt +++ b/rosbag2_storage_mcap/CMakeLists.txt @@ -24,6 +24,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(ament_index_cpp REQUIRED) +find_package(rclcpp REQUIRED) find_package(mcap_vendor REQUIRED) find_package(pluginlib REQUIRED) find_package(rcutils REQUIRED) @@ -43,6 +44,7 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "ROSBAG2_STORAGE_MCAP_BUILDIN target_link_libraries(${PROJECT_NAME} mcap_vendor::mcap pluginlib::pluginlib + rclcpp::rclcpp rcutils::rcutils rosbag2_storage::rosbag2_storage ) diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index 716967a12b..8a35d652f9 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -733,7 +733,7 @@ void MCAPStorage::write(std::shared_ptrsecond; mcap_msg.sequence = 0; if (msg->time_stamp < 0) { - RCUTILS_LOG_WARN_NAMED(LOG_NAME, "Invalid message timestamp %ld", msg->time_stamp); + RCUTILS_LOG_WARN_NAMED(LOG_NAME, "Invalid message timestamp %lld", msg->time_stamp); } mcap_msg.logTime = mcap::Timestamp(msg->time_stamp); mcap_msg.publishTime = mcap_msg.logTime; diff --git a/rosbag2_storage_sqlite3/CMakeLists.txt b/rosbag2_storage_sqlite3/CMakeLists.txt index 2c13fcf488..8df9a97cef 100644 --- a/rosbag2_storage_sqlite3/CMakeLists.txt +++ b/rosbag2_storage_sqlite3/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(pluginlib REQUIRED) find_package(rcpputils REQUIRED) +find_package(rclcpp REQUIRED) find_package(rcutils REQUIRED) find_package(rosbag2_storage REQUIRED) find_package(sqlite3_vendor REQUIRED) @@ -42,6 +43,7 @@ target_link_libraries(${PROJECT_NAME} pluginlib::pluginlib rosbag2_storage::rosbag2_storage rcpputils::rcpputils + rclcpp::rclcpp rcutils::rcutils SQLite::SQLite3 yaml-cpp From c07d785eef966585d2b4628d5363dc5ac49d01f2 Mon Sep 17 00:00:00 2001 From: Peter Favrholdt Date: Sun, 3 Sep 2023 17:49:10 +0200 Subject: [PATCH 02/13] fix linelength and whitespace Signed-off-by: Peter Favrholdt --- .../src/rosbag2_cpp/writers/sequential_writer.cpp | 14 +++++++++----- rosbag2_py/src/rosbag2_py/_storage.cpp | 4 ++-- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 86d41e4bdf..708c2c44e7 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -354,7 +354,8 @@ void SequentialWriter::write(std::shared_ptr( std::chrono::nanoseconds(message->time_stamp)); - if (! message_within_accepted_time_range(message_timestamp)) return; + if (!message_within_accepted_time_range(message_timestamp)) + return; if (is_first_message_) { // Update bagfile starting time @@ -434,14 +435,17 @@ bool SequentialWriter::should_split_bagfile( return should_split; } -bool SequentialWriter::message_within_accepted_time_range(const std::chrono::time_point ¤t_time) const +bool SequentialWriter::message_within_accepted_time_range( + const std::chrono::time_point ¤t_time) const { - bool retVal=true; + bool retVal = true; if ( (storage_options_.start_time > 0) && - current_time < std::chrono::time_point(std::chrono::nanoseconds(storage_options_.start_time)) ) + current_time < std::chrono::time_point( + std::chrono::nanoseconds(storage_options_.start_time)) ) retVal = false; else if ( ( storage_options_.end_time > 0) && - current_time > std::chrono::time_point(std::chrono::nanoseconds(storage_options_.end_time)) ) + current_time > std::chrono::time_point( + std::chrono::nanoseconds(storage_options_.end_time)) ) retVal = false; return retVal; } diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index 509581bf29..e498f96f60 100644 --- a/rosbag2_py/src/rosbag2_py/_storage.cpp +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -83,8 +83,8 @@ PYBIND11_MODULE(_storage, m) { pybind11::class_(m, "StorageOptions") .def( pybind11::init< - std::string, std::string, uint64_t, uint64_t, uint64_t, std::string, std::string, bool, rcutils_time_point_value_t, rcutils_time_point_value_t, - KEY_VALUE_MAP>(), + std::string, std::string, uint64_t, uint64_t, uint64_t, std::string, std::string, bool, + rcutils_time_point_value_t, rcutils_time_point_value_t, KEY_VALUE_MAP>(), pybind11::arg("uri"), pybind11::arg("storage_id") = "", pybind11::arg("max_bagfile_size") = 0, From 134daba4a3e6028da1ff5fb46595cee7aaafd6c9 Mon Sep 17 00:00:00 2001 From: Peter Favrholdt Date: Sun, 3 Sep 2023 20:28:43 +0200 Subject: [PATCH 03/13] minor fix warning %lld versus long int Signed-off-by: Peter Favrholdt --- rosbag2_storage_mcap/src/mcap_storage.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbag2_storage_mcap/src/mcap_storage.cpp b/rosbag2_storage_mcap/src/mcap_storage.cpp index 8a35d652f9..716967a12b 100644 --- a/rosbag2_storage_mcap/src/mcap_storage.cpp +++ b/rosbag2_storage_mcap/src/mcap_storage.cpp @@ -733,7 +733,7 @@ void MCAPStorage::write(std::shared_ptrsecond; mcap_msg.sequence = 0; if (msg->time_stamp < 0) { - RCUTILS_LOG_WARN_NAMED(LOG_NAME, "Invalid message timestamp %lld", msg->time_stamp); + RCUTILS_LOG_WARN_NAMED(LOG_NAME, "Invalid message timestamp %ld", msg->time_stamp); } mcap_msg.logTime = mcap::Timestamp(msg->time_stamp); mcap_msg.publishTime = mcap_msg.logTime; From d5295cc867fdb41bc4b36b2282a9e4e4bebbca4f Mon Sep 17 00:00:00 2001 From: Peter Favrholdt Date: Sun, 3 Sep 2023 21:58:52 +0200 Subject: [PATCH 04/13] Minor whitespace uncrustify fix Signed-off-by: Peter Favrholdt --- .../rosbag2_cpp/writers/sequential_writer.cpp | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 708c2c44e7..0a6f302e5c 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -354,8 +354,9 @@ void SequentialWriter::write(std::shared_ptr( std::chrono::nanoseconds(message->time_stamp)); - if (!message_within_accepted_time_range(message_timestamp)) + if (!message_within_accepted_time_range(message_timestamp)) { return; + } if (is_first_message_) { // Update bagfile starting time @@ -436,17 +437,20 @@ bool SequentialWriter::should_split_bagfile( } bool SequentialWriter::message_within_accepted_time_range( - const std::chrono::time_point ¤t_time) const + const std::chrono::time_point & current_time) const { bool retVal = true; if ( (storage_options_.start_time > 0) && - current_time < std::chrono::time_point( - std::chrono::nanoseconds(storage_options_.start_time)) ) - retVal = false; - else if ( ( storage_options_.end_time > 0) && - current_time > std::chrono::time_point( - std::chrono::nanoseconds(storage_options_.end_time)) ) - retVal = false; + current_time < std::chrono::time_point( + std::chrono::nanoseconds(storage_options_.start_time)) ) + { + retVal = false; + } else if ( ( storage_options_.end_time > 0) && + current_time > std::chrono::time_point( + std::chrono::nanoseconds(storage_options_.end_time)) ) + { + retVal = false; + } return retVal; } From f93333239c673a9c7db87196c61f891f6a7bcdc6 Mon Sep 17 00:00:00 2001 From: Peter Favrholdt Date: Sun, 3 Sep 2023 22:34:41 +0200 Subject: [PATCH 05/13] minor cpplint whitespace/indent change Signed-off-by: Peter Favrholdt --- rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 0a6f302e5c..88df52799d 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -445,11 +445,13 @@ bool SequentialWriter::message_within_accepted_time_range( std::chrono::nanoseconds(storage_options_.start_time)) ) { retVal = false; - } else if ( ( storage_options_.end_time > 0) && + } else { + if ( (storage_options_.end_time > 0) && current_time > std::chrono::time_point( std::chrono::nanoseconds(storage_options_.end_time)) ) - { - retVal = false; + { + retVal = false; + } } return retVal; } From aeb07d356ef7f3a5a9990d391026b4f9549e9362 Mon Sep 17 00:00:00 2001 From: Peter Favrholdt Date: Sun, 3 Sep 2023 23:01:11 +0200 Subject: [PATCH 06/13] minor whitespace fix Signed-off-by: Peter Favrholdt --- rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 88df52799d..005b627a20 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -447,8 +447,8 @@ bool SequentialWriter::message_within_accepted_time_range( retVal = false; } else { if ( (storage_options_.end_time > 0) && - current_time > std::chrono::time_point( - std::chrono::nanoseconds(storage_options_.end_time)) ) + current_time > std::chrono::time_point( + std::chrono::nanoseconds(storage_options_.end_time)) ) { retVal = false; } From 87e99bd046b6809c593ef9cc4f76e4b5f600d655 Mon Sep 17 00:00:00 2001 From: Peter Favrholdt Date: Mon, 25 Sep 2023 21:06:24 +0200 Subject: [PATCH 07/13] Remove dependency on rclcpp for sequential_writer by using uint64_t. Moved timestamp check above Get TopicInformation Added parsing test. Signed-off-by: Peter Favrholdt --- .../rosbag2_cpp/writers/sequential_writer.hpp | 1 + .../rosbag2_cpp/writers/sequential_writer.cpp | 30 +++++++++---------- rosbag2_py/src/rosbag2_py/_storage.cpp | 14 ++++----- rosbag2_storage/CMakeLists.txt | 2 -- .../rosbag2_storage/storage_options.hpp | 5 ++-- .../src/rosbag2_storage/storage_options.cpp | 8 ++--- .../rosbag2_storage/test_storage_options.cpp | 4 +++ 7 files changed, 33 insertions(+), 31 deletions(-) diff --git a/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp b/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp index cd5e27cf69..0787868ff1 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 timestamp is within start_time_ns and end_time_ns. * * \param message to be written to the bagfile * \throws runtime_error if the Writer is not open. diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 005b627a20..2ca9ecd6d2 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -340,6 +340,13 @@ void SequentialWriter::write(std::shared_ptr( + std::chrono::nanoseconds(message->time_stamp)); + + if (!message_within_accepted_time_range(message_timestamp)) { + return; + } + // Get TopicInformation handler for counting messages. rosbag2_storage::TopicInformation * topic_information {nullptr}; try { @@ -351,13 +358,6 @@ void SequentialWriter::write(std::shared_ptr( - std::chrono::nanoseconds(message->time_stamp)); - - if (!message_within_accepted_time_range(message_timestamp)) { - return; - } - if (is_first_message_) { // Update bagfile starting time metadata_.starting_time = message_timestamp; @@ -439,21 +439,21 @@ bool SequentialWriter::should_split_bagfile( bool SequentialWriter::message_within_accepted_time_range( const std::chrono::time_point & current_time) const { - bool retVal = true; - if ( (storage_options_.start_time > 0) && + bool ret_value = true; + if ( (storage_options_.start_time_ns > 0) && current_time < std::chrono::time_point( - std::chrono::nanoseconds(storage_options_.start_time)) ) + std::chrono::nanoseconds(storage_options_.start_time_ns)) ) { - retVal = false; + ret_value = false; } else { - if ( (storage_options_.end_time > 0) && + if ( (storage_options_.end_time_ns > 0) && current_time > std::chrono::time_point( - std::chrono::nanoseconds(storage_options_.end_time)) ) + std::chrono::nanoseconds(storage_options_.end_time_ns)) ) { - retVal = false; + ret_value = false; } } - return retVal; + return ret_value; } void SequentialWriter::finalize_metadata() diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index e498f96f60..a75c4412fc 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, - rcutils_time_point_value_t, rcutils_time_point_value_t, KEY_VALUE_MAP>(), + uint64_t, uint64_t, KEY_VALUE_MAP>(), pybind11::arg("uri"), pybind11::arg("storage_id") = "", pybind11::arg("max_bagfile_size") = 0, @@ -93,8 +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") = 0, - pybind11::arg("end_time") = 0, + pybind11::arg("start_time_ns") = 0, + pybind11::arg("end_time_ns") = 0, pybind11::arg("custom_data") = KEY_VALUE_MAP{}) .def_readwrite("uri", &rosbag2_storage::StorageOptions::uri) .def_readwrite("storage_id", &rosbag2_storage::StorageOptions::storage_id) @@ -117,11 +117,11 @@ PYBIND11_MODULE(_storage, m) { "snapshot_mode", &rosbag2_storage::StorageOptions::snapshot_mode) .def_readwrite( - "start_time", - &rosbag2_storage::StorageOptions::start_time) + "start_time_ns", + &rosbag2_storage::StorageOptions::start_time_ns) .def_readwrite( - "end_time", - &rosbag2_storage::StorageOptions::end_time) + "end_time_ns", + &rosbag2_storage::StorageOptions::end_time_ns) .def_readwrite( "custom_data", &rosbag2_storage::StorageOptions::custom_data); diff --git a/rosbag2_storage/CMakeLists.txt b/rosbag2_storage/CMakeLists.txt index 3d61538095..cd4d7ab601 100644 --- a/rosbag2_storage/CMakeLists.txt +++ b/rosbag2_storage/CMakeLists.txt @@ -22,7 +22,6 @@ if(WIN32) endif() find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) find_package(pluginlib REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) @@ -45,7 +44,6 @@ target_include_directories(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} pluginlib::pluginlib rcpputils::rcpputils - rclcpp::rclcpp rcutils::rcutils yaml-cpp ) diff --git a/rosbag2_storage/include/rosbag2_storage/storage_options.hpp b/rosbag2_storage/include/rosbag2_storage/storage_options.hpp index c1d8387f17..bd81a23052 100644 --- a/rosbag2_storage/include/rosbag2_storage/storage_options.hpp +++ b/rosbag2_storage/include/rosbag2_storage/storage_options.hpp @@ -21,7 +21,6 @@ #include "rosbag2_storage/visibility_control.hpp" #include "rosbag2_storage/yaml.hpp" -#include "rclcpp/time.hpp" namespace rosbag2_storage { @@ -58,8 +57,8 @@ struct StorageOptions bool snapshot_mode = false; // Start and end time for cutting - rcutils_time_point_value_t start_time = 0; - rcutils_time_point_value_t end_time = 0; + uint64_t start_time_ns = 0; + uint64_t end_time_ns = 0; // 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 1f2cabd09d..abf78e3c9e 100644 --- a/rosbag2_storage/src/rosbag2_storage/storage_options.cpp +++ b/rosbag2_storage/src/rosbag2_storage/storage_options.cpp @@ -32,8 +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"] = storage_options.start_time; - node["end_time"] = storage_options.end_time; + 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; } @@ -50,8 +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", storage_options.start_time); - optional_assign(node, "end_time", storage_options.end_time); + 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); } From d24566a4ae05a0995c93f7e53fbd38e943f0dbfc Mon Sep 17 00:00:00 2001 From: Peter Favrholdt Date: Wed, 27 Sep 2023 11:03:16 +0200 Subject: [PATCH 08/13] Remove superfluous dependency on rclcpp. Signed-off-by: Peter Favrholdt --- rosbag2_storage_mcap/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/rosbag2_storage_mcap/CMakeLists.txt b/rosbag2_storage_mcap/CMakeLists.txt index 441ca44ad3..f19e9f3446 100644 --- a/rosbag2_storage_mcap/CMakeLists.txt +++ b/rosbag2_storage_mcap/CMakeLists.txt @@ -24,7 +24,6 @@ endif() find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(ament_index_cpp REQUIRED) -find_package(rclcpp REQUIRED) find_package(mcap_vendor REQUIRED) find_package(pluginlib REQUIRED) find_package(rcutils REQUIRED) @@ -44,7 +43,6 @@ target_compile_definitions(${PROJECT_NAME} PRIVATE "ROSBAG2_STORAGE_MCAP_BUILDIN target_link_libraries(${PROJECT_NAME} mcap_vendor::mcap pluginlib::pluginlib - rclcpp::rclcpp rcutils::rcutils rosbag2_storage::rosbag2_storage ) From f431f3870ab75262b98c01216d1269ffa1da727b Mon Sep 17 00:00:00 2001 From: Peter Favrholdt Date: Wed, 27 Sep 2023 11:04:56 +0200 Subject: [PATCH 09/13] Add note that start_time_ns and end_time_ns are from storage_options. Signed-off-by: Peter Favrholdt --- rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp b/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp index 0787868ff1..d36e09a2ee 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp @@ -117,7 +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 timestamp is within start_time_ns and end_time_ns. + * 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. From f26a0bbd595a116a033aa36faf7343dac6414263 Mon Sep 17 00:00:00 2001 From: Peter Favrholdt Date: Wed, 27 Sep 2023 15:13:37 +0200 Subject: [PATCH 10/13] Simplified message_within_accepted_time_range now uses int64_t. Signed-off-by: Peter Favrholdt --- .../rosbag2_cpp/writers/sequential_writer.hpp | 2 +- .../rosbag2_cpp/writers/sequential_writer.cpp | 44 +++++++++---------- rosbag2_py/src/rosbag2_py/_storage.cpp | 6 +-- .../rosbag2_storage/storage_options.hpp | 4 +- .../src/rosbag2_storage/storage_options.cpp | 4 +- 5 files changed, 29 insertions(+), 31 deletions(-) diff --git a/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp b/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp index d36e09a2ee..1695203185 100644 --- a/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp +++ b/rosbag2_cpp/include/rosbag2_cpp/writers/sequential_writer.hpp @@ -177,7 +177,7 @@ class ROSBAG2_CPP_PUBLIC SequentialWriter // Checks if the message to be written is within accepted time range bool message_within_accepted_time_range( - const std::chrono::time_point & current_time) const; + 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 2ca9ecd6d2..92f16b2c1a 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -340,10 +340,7 @@ void SequentialWriter::write(std::shared_ptr( - std::chrono::nanoseconds(message->time_stamp)); - - if (!message_within_accepted_time_range(message_timestamp)) { + if (!message_within_accepted_time_range(message->time_stamp)) { return; } @@ -358,6 +355,9 @@ void SequentialWriter::write(std::shared_ptr( + std::chrono::nanoseconds(message->time_stamp)); + if (is_first_message_) { // Update bagfile starting time metadata_.starting_time = message_timestamp; @@ -436,25 +436,23 @@ bool SequentialWriter::should_split_bagfile( return should_split; } -bool SequentialWriter::message_within_accepted_time_range( - const std::chrono::time_point & current_time) const -{ - bool ret_value = true; - if ( (storage_options_.start_time_ns > 0) && - current_time < std::chrono::time_point( - std::chrono::nanoseconds(storage_options_.start_time_ns)) ) - { - ret_value = false; - } else { - if ( (storage_options_.end_time_ns > 0) && - current_time > std::chrono::time_point( - std::chrono::nanoseconds(storage_options_.end_time_ns)) ) - { - ret_value = false; - } - } - return ret_value; -} +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() { diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index a75c4412fc..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, - uint64_t, uint64_t, KEY_VALUE_MAP>(), + int64_t, int64_t, KEY_VALUE_MAP>(), pybind11::arg("uri"), pybind11::arg("storage_id") = "", pybind11::arg("max_bagfile_size") = 0, @@ -93,8 +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") = 0, - pybind11::arg("end_time_ns") = 0, + 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) diff --git a/rosbag2_storage/include/rosbag2_storage/storage_options.hpp b/rosbag2_storage/include/rosbag2_storage/storage_options.hpp index bd81a23052..8d6d188459 100644 --- a/rosbag2_storage/include/rosbag2_storage/storage_options.hpp +++ b/rosbag2_storage/include/rosbag2_storage/storage_options.hpp @@ -57,8 +57,8 @@ struct StorageOptions bool snapshot_mode = false; // Start and end time for cutting - uint64_t start_time_ns = 0; - uint64_t end_time_ns = 0; + 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 abf78e3c9e..717fcb9082 100644 --- a/rosbag2_storage/src/rosbag2_storage/storage_options.cpp +++ b/rosbag2_storage/src/rosbag2_storage/storage_options.cpp @@ -50,8 +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); + 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; From 15da4624b417f95b5762b3cd7b97e4e09d7d681e Mon Sep 17 00:00:00 2001 From: Peter Favrholdt Date: Wed, 27 Sep 2023 17:28:29 +0200 Subject: [PATCH 11/13] whitespace fixes only Signed-off-by: Peter Favrholdt --- .../rosbag2_cpp/writers/sequential_writer.cpp | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 92f16b2c1a..7cd55bca35 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -436,23 +436,23 @@ 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; -} +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() { From d660fe6689a9e9982fb9841f04a0badb21dd3816 Mon Sep 17 00:00:00 2001 From: Peter Favrholdt Date: Wed, 27 Sep 2023 17:51:07 +0200 Subject: [PATCH 12/13] minor whitespace change Signed-off-by: Peter Favrholdt --- rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 7cd55bca35..2bbafa689b 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -440,13 +440,13 @@ 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) + 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) + static_cast(current_time) > storage_options_.end_time_ns) { return false; } From 9bb32053ba2924fb78edca220fd77d2ac72b431a Mon Sep 17 00:00:00 2001 From: Peter Favrholdt Date: Thu, 28 Sep 2023 11:08:16 +0200 Subject: [PATCH 13/13] remove superfluous dependency on rclcpp from storage_sqlite3 Signed-off-by: Peter Favrholdt --- rosbag2_storage_sqlite3/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/rosbag2_storage_sqlite3/CMakeLists.txt b/rosbag2_storage_sqlite3/CMakeLists.txt index 8df9a97cef..2c13fcf488 100644 --- a/rosbag2_storage_sqlite3/CMakeLists.txt +++ b/rosbag2_storage_sqlite3/CMakeLists.txt @@ -25,7 +25,6 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_python REQUIRED) find_package(pluginlib REQUIRED) find_package(rcpputils REQUIRED) -find_package(rclcpp REQUIRED) find_package(rcutils REQUIRED) find_package(rosbag2_storage REQUIRED) find_package(sqlite3_vendor REQUIRED) @@ -43,7 +42,6 @@ target_link_libraries(${PROJECT_NAME} pluginlib::pluginlib rosbag2_storage::rosbag2_storage rcpputils::rcpputils - rclcpp::rclcpp rcutils::rcutils SQLite::SQLite3 yaml-cpp