ros2 bag convert now excludes messages not in [start_time;end_time]

resolves ros2#599

Signed-off-by: Peter Favrholdt <ros2github@how.dk>

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<std::chrono::high_resolution_clock> & 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<std::chrono::high_resolution_clock> & current_time) const;
+
   // Prepares the metadata by setting initial values.
   virtual void init_metadata();
 

rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp

@@ -354,6 +354,8 @@ void SequentialWriter::write(std::shared_ptr<const rosbag2_storage::SerializedBa
   const auto message_timestamp = std::chrono::time_point<std::chrono::high_resolution_clock>(
     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<std::chrono::high_resolution_clock> &current_time) const
+{
+  bool retVal=true;
+  if ( (storage_options_.start_time > 0) &&
+        current_time < std::chrono::time_point<std::chrono::high_resolution_clock>(std::chrono::nanoseconds(storage_options_.start_time)) )
+          retVal = false;
+  else if ( ( storage_options_.end_time > 0) &&
+        current_time > std::chrono::time_point<std::chrono::high_resolution_clock>(std::chrono::nanoseconds(storage_options_.end_time)) )
+          retVal = false;
+  return retVal;
+}
+
 void SequentialWriter::finalize_metadata()
 {
   metadata_.bag_size = 0;

rosbag2_py/src/rosbag2_py/_storage.cpp

@@ -83,7 +83,7 @@ PYBIND11_MODULE(_storage, m) {
   pybind11::class_<rosbag2_storage::StorageOptions>(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);

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
 )

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<std::string, std::string> custom_data{};
 };

rosbag2_storage/src/rosbag2_storage/storage_options.cpp

@@ -32,6 +32,8 @@ Node convert<rosbag2_storage::StorageOptions>::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<rosbag2_storage::StorageOptions>::decode(
     node, "storage_preset_profile", storage_options.storage_preset_profile);
   optional_assign<std::string>(node, "storage_config_uri", storage_options.storage_config_uri);
   optional_assign<bool>(node, "snapshot_mode", storage_options.snapshot_mode);
+  optional_assign<rcutils_time_point_value_t>(node, "start_time", storage_options.start_time);
+  optional_assign<rcutils_time_point_value_t>(node, "end_time", storage_options.end_time);
   using KEY_VALUE_MAP = std::unordered_map<std::string, std::string>;
   optional_assign<KEY_VALUE_MAP>(node, "custom_data", storage_options.custom_data);
   return true;

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
 )

rosbag2_storage_mcap/src/mcap_storage.cpp

@@ -733,7 +733,7 @@ void MCAPStorage::write(std::shared_ptr<const rosbag2_storage::SerializedBagMess
   mcap_msg.channelId = channel_it->second;
   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;

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