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

…1455) * ros2 bag convert now excludes messages not in [start_time;end_time] resolves #599 Signed-off-by: Peter Favrholdt <ros2github@how.dk> * fix linelength and whitespace Signed-off-by: Peter Favrholdt <ros2github@how.dk> * minor fix warning %lld versus long int Signed-off-by: Peter Favrholdt <ros2github@how.dk> * Minor whitespace uncrustify fix Signed-off-by: Peter Favrholdt <ros2github@how.dk> * minor cpplint whitespace/indent change Signed-off-by: Peter Favrholdt <ros2github@how.dk> * minor whitespace fix Signed-off-by: Peter Favrholdt <ros2github@how.dk> * 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 <ros2github@how.dk> * Remove superfluous dependency on rclcpp. Signed-off-by: Peter Favrholdt <ros2github@how.dk> * Add note that start_time_ns and end_time_ns are from storage_options. Signed-off-by: Peter Favrholdt <ros2github@how.dk> * Simplified message_within_accepted_time_range now uses int64_t. Signed-off-by: Peter Favrholdt <ros2github@how.dk> * whitespace fixes only Signed-off-by: Peter Favrholdt <ros2github@how.dk> * minor whitespace change Signed-off-by: Peter Favrholdt <ros2github@how.dk> * remove superfluous dependency on rclcpp from storage_sqlite3 Signed-off-by: Peter Favrholdt <ros2github@how.dk> --------- Signed-off-by: Peter Favrholdt <ros2github@how.dk>
ros2 · Sep 28, 2023 · a80adfd · a80adfd
1 parent a029ef9
commit a80adfd
Show file tree

Hide file tree

Showing 6 changed files with 48 additions and 1 deletion.
diff --git 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<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 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
@@ -340,6 +340,10 @@ void SequentialWriter::write(std::shared_ptr<const rosbag2_storage::SerializedBa
     throw std::runtime_error("Bag is not open. Call open() before writing.");
   }
 
+  if (!message_within_accepted_time_range(message->time_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<int64_t>(current_time) < storage_options_.start_time_ns)
+  {
+    return false;
+  }
+
+  if (storage_options_.end_time_ns >= 0 &&
+    static_cast<int64_t>(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
@@ -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
@@ -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<std::string, std::string> custom_data{};
 };

diff --git a/rosbag2_storage/src/rosbag2_storage/storage_options.cpp b/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_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<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<int64_t>(node, "start_time_ns", storage_options.start_time_ns);
+  optional_assign<int64_t>(node, "end_time_ns", storage_options.end_time_ns);
   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;

diff --git 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);
 }