From 7986ede9f0715df4815a9a7d4b3e440713f67fe0 Mon Sep 17 00:00:00 2001 From: Michael Orlov Date: Mon, 28 Oct 2024 10:17:29 -0700 Subject: [PATCH] Change type for snapshot_duration to the rclcpp::Duration Signed-off-by: Michael Orlov --- ros2bag/ros2bag/verb/record.py | 7 ++- .../rosbag2_cpp/writers/sequential_writer.cpp | 5 +- rosbag2_py/rosbag2_py/_storage.pyi | 4 +- rosbag2_py/src/rosbag2_py/_storage.cpp | 57 ++++++++++++++++--- .../rosbag2_storage/storage_options.hpp | 7 ++- .../src/rosbag2_storage/storage_options.cpp | 3 +- .../rosbag2_storage/test_storage_options.cpp | 2 +- 7 files changed, 65 insertions(+), 20 deletions(-) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 3260ee474..92eab4863 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -18,6 +18,7 @@ from rclpy.qos import InvalidQoSProfileException from ros2bag.api import add_writer_storage_plugin_extensions +from ros2bag.api import check_not_negative_float from ros2bag.api import convert_service_to_service_event_topic from ros2bag.api import convert_yaml_to_qos_profile from ros2bag.api import print_error @@ -177,8 +178,8 @@ def add_recorder_arguments(parser: ArgumentParser) -> None: 'the "/rosbag2_recorder/snapshot" service is called. e.g. \n ' 'ros2 service call /rosbag2_recorder/snapshot rosbag2_interfaces/Snapshot') parser.add_argument( - '--snapshot-duration', type=int, default=0, - help='Maximum snapshot duration in milliseconds.\n' + '--snapshot-duration', type=check_not_negative_float, default=0.0, + help='Maximum snapshot duration in a fraction of seconds.\n' 'Default: %(default)d, indicates that the snapshot will be limited by the' ' --max-cache-size parameter only. If the value is more than 0, the cyclic buffer' ' for the snapshot will be limited by both the series of messages duration and the' @@ -290,7 +291,7 @@ def validate_parsed_arguments(args, uri) -> str: if args.compression_queue_size < 0: return print_error('Compression queue size must be at least 0.') - if args.snapshot_mode and args.snapshot_duration == 0 and args.max_cache_size == 0: + if args.snapshot_mode and args.snapshot_duration == 0.0 and args.max_cache_size == 0: return print_error('In snapshot mode, either the snapshot_duration or max_bytes_size shall' ' not be set to zero.') diff --git a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp index 87397262b..51ec0c2e5 100644 --- a/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp +++ b/rosbag2_cpp/src/rosbag2_cpp/writers/sequential_writer.cpp @@ -146,7 +146,7 @@ void SequentialWriter::open( } use_cache_ = storage_options.max_cache_size > 0u || - (storage_options.snapshot_mode && storage_options.snapshot_duration > 0); + (storage_options.snapshot_mode && storage_options.snapshot_duration.nanoseconds() > 0); if (storage_options.snapshot_mode && !use_cache_) { throw std::runtime_error( "Either the max cache size or the maximum snapshot duration must be greater than 0" @@ -155,9 +155,8 @@ void SequentialWriter::open( if (use_cache_) { if (storage_options.snapshot_mode) { - int64_t max_buffer_duration_ns = storage_options.snapshot_duration * 1000000; message_cache_ = std::make_shared( - storage_options.max_cache_size, max_buffer_duration_ns); + storage_options.max_cache_size, storage_options.snapshot_duration.nanoseconds()); } else { message_cache_ = std::make_shared( storage_options.max_cache_size); diff --git a/rosbag2_py/rosbag2_py/_storage.pyi b/rosbag2_py/rosbag2_py/_storage.pyi index 1ff573c61..52d3ff49b 100644 --- a/rosbag2_py/rosbag2_py/_storage.pyi +++ b/rosbag2_py/rosbag2_py/_storage.pyi @@ -101,14 +101,14 @@ class StorageOptions: max_bagfile_duration: int max_bagfile_size: int max_cache_size: int - snapshot_duration: int + snapshot_duration: float snapshot_mode: bool start_time_ns: int storage_config_uri: str storage_id: str storage_preset_profile: str uri: str - def __init__(self, uri: str, storage_id: str = ..., max_bagfile_size: int = ..., max_bagfile_duration: int = ..., max_cache_size: int = ..., storage_preset_profile: str = ..., storage_config_uri: str = ..., snapshot_mode: bool = ..., snapshot_duration: int = ..., start_time_ns: int = ..., end_time_ns: int = ..., custom_data: Dict[str, str] = ...) -> None: ... + def __init__(self, uri: str, storage_id: str = ..., max_bagfile_size: int = ..., max_bagfile_duration: int = ..., max_cache_size: int = ..., storage_preset_profile: str = ..., storage_config_uri: str = ..., snapshot_mode: bool = ..., snapshot_duration: Duration = ..., start_time_ns: int = ..., end_time_ns: int = ..., custom_data: Dict[str, str] = ...) -> None: ... class TopicInformation: message_count: int diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index 9abf1b382..4739db640 100644 --- a/rosbag2_py/src/rosbag2_py/_storage.cpp +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include "rosbag2_cpp/converter_options.hpp" @@ -44,6 +45,16 @@ std::chrono::nanoseconds from_rclpy_duration(const pybind11::object & duration) return std::chrono::nanoseconds(nanos); } +pybind11::object rclcpp_duration_to_py_float(const rclcpp::Duration & duration) +{ + return pybind11::cast(duration.seconds()); +} + +rclcpp::Duration py_float_to_rclcpp_duration(const pybind11::object & obj) +{ + return rclcpp::Duration::from_seconds(obj.cast()); +} + template pybind11::object to_rclpy_time(T time) { @@ -81,9 +92,36 @@ PYBIND11_MODULE(_storage, m) { using KEY_VALUE_MAP = std::unordered_map; pybind11::class_(m, "StorageOptions") .def( - pybind11::init< - std::string, std::string, uint64_t, uint64_t, uint64_t, std::string, std::string, bool, - int64_t, int64_t, int64_t, KEY_VALUE_MAP>(), + pybind11::init( + []( + std::string uri, + std::string storage_id, + uint64_t max_bagfile_size, + uint64_t max_bagfile_duration, + uint64_t max_cache_size, + std::string storage_preset_profile, + std::string storage_config_uri, + bool snapshot_mode, + const pybind11::object & snapshot_duration, + int64_t start_time_ns, + int64_t end_time_ns, + KEY_VALUE_MAP custom_data) + { + return rosbag2_storage::StorageOptions{ + std::move(uri), + std::move(storage_id), + max_bagfile_size, + max_bagfile_duration, + max_cache_size, + std::move(storage_preset_profile), + std::move(storage_config_uri), + snapshot_mode, + py_float_to_rclcpp_duration(snapshot_duration), + start_time_ns, + end_time_ns, + std::move(custom_data), + }; + }), pybind11::arg("uri"), pybind11::arg("storage_id") = "", pybind11::arg("max_bagfile_size") = 0, @@ -92,7 +130,7 @@ PYBIND11_MODULE(_storage, m) { pybind11::arg("storage_preset_profile") = "", pybind11::arg("storage_config_uri") = "", pybind11::arg("snapshot_mode") = false, - pybind11::arg("snapshot_duration") = 0, + pybind11::arg("snapshot_duration") = rclcpp_duration_to_py_float(rclcpp::Duration(0, 0)), pybind11::arg("start_time_ns") = -1, pybind11::arg("end_time_ns") = -1, pybind11::arg("custom_data") = KEY_VALUE_MAP{}) @@ -116,9 +154,14 @@ PYBIND11_MODULE(_storage, m) { .def_readwrite( "snapshot_mode", &rosbag2_storage::StorageOptions::snapshot_mode) - .def_readwrite( - "snapshot_duration", - &rosbag2_storage::StorageOptions::snapshot_duration) + .def_property( + "snapshot_duration", + [](const rosbag2_storage::StorageOptions & self) { + return rclcpp_duration_to_py_float(self.snapshot_duration); + }, + [](rosbag2_storage::StorageOptions & self, const pybind11::object & obj) { + self.snapshot_duration = py_float_to_rclcpp_duration(obj); + }) .def_readwrite( "start_time_ns", &rosbag2_storage::StorageOptions::start_time_ns) diff --git a/rosbag2_storage/include/rosbag2_storage/storage_options.hpp b/rosbag2_storage/include/rosbag2_storage/storage_options.hpp index 00f51cd2b..68f652f74 100644 --- a/rosbag2_storage/include/rosbag2_storage/storage_options.hpp +++ b/rosbag2_storage/include/rosbag2_storage/storage_options.hpp @@ -19,6 +19,7 @@ #include #include +#include "rclcpp/duration.hpp" #include "rosbag2_storage/visibility_control.hpp" #include "rosbag2_storage/yaml.hpp" @@ -56,9 +57,9 @@ struct StorageOptions // Defaults to disabled. bool snapshot_mode = false; - // The maximum snapshot duration in milliseconds. - // A value of 0 indicates that snapshot will be limited by the max_cache_size only. - int64_t snapshot_duration = 0; + // The maximum snapshot duration. + // A value of 0.0 indicates that snapshot will be limited by the max_cache_size only. + rclcpp::Duration snapshot_duration{0, 0}; // Start and end time for cutting int64_t start_time_ns = -1; diff --git a/rosbag2_storage/src/rosbag2_storage/storage_options.cpp b/rosbag2_storage/src/rosbag2_storage/storage_options.cpp index af63544ee..fbd37b91b 100644 --- a/rosbag2_storage/src/rosbag2_storage/storage_options.cpp +++ b/rosbag2_storage/src/rosbag2_storage/storage_options.cpp @@ -15,6 +15,7 @@ #include #include +#include "rclcpp/duration.hpp" #include "rosbag2_storage/storage_options.hpp" namespace YAML @@ -51,7 +52,7 @@ 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, "snapshot_duration", storage_options.snapshot_duration); + optional_assign(node, "snapshot_duration", storage_options.snapshot_duration); 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; diff --git a/rosbag2_storage/test/rosbag2_storage/test_storage_options.cpp b/rosbag2_storage/test/rosbag2_storage/test_storage_options.cpp index f36000781..116296de2 100644 --- a/rosbag2_storage/test/rosbag2_storage/test_storage_options.cpp +++ b/rosbag2_storage/test/rosbag2_storage/test_storage_options.cpp @@ -29,7 +29,7 @@ TEST(storage_options, test_yaml_serialization) original.storage_preset_profile = "profile"; original.storage_config_uri = "config_uri"; original.snapshot_mode = true; - original.snapshot_duration = 1500; + original.snapshot_duration = rclcpp::Duration::from_seconds(1.5); original.start_time_ns = 12345000; original.end_time_ns = 23456000; original.custom_data["key1"] = "value1";