diff --git a/rosbag2_py/CMakeLists.txt b/rosbag2_py/CMakeLists.txt index e72dd233e9..91831730ac 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -118,12 +118,13 @@ if(BUILD_TESTING) set(set_env_vars "${set_env_vars} ROSBAG2_PY_TEST_WITH_RTLD_GLOBAL=True") endif() - # message(FATAL_ERROR "HELP I AM THE PATH ${CMAKE_CURRENT_SOURCE_DIR}") - ament_add_pytest_test(test_sequential_reader_py "test/test_sequential_reader.py" + ament_add_pytest_test(test_sequential_reader_py + "test/test_sequential_reader.py" APPEND_ENV "${append_env_vars}" ENV "${set_env_vars}" ) - ament_add_pytest_test(test_sequential_reader_multiple_files_py "test/test_sequential_reader_multiple_files.py" + ament_add_pytest_test(test_sequential_reader_multiple_files_py + "test/test_sequential_reader_multiple_files.py" APPEND_ENV "${append_env_vars}" ENV "${set_env_vars}" ) @@ -147,6 +148,12 @@ if(BUILD_TESTING) APPEND_ENV "${append_env_vars}" ENV "${set_env_vars}" ) + + ament_add_pytest_test(test_storage_py + "test/test_storage.py" + APPEND_ENV "${append_env_vars}" + ENV "${set_env_vars}" + ) endif() ament_package() diff --git a/rosbag2_py/rosbag2_py/__init__.py b/rosbag2_py/rosbag2_py/__init__.py index 82da7d3147..193944fef8 100644 --- a/rosbag2_py/rosbag2_py/__init__.py +++ b/rosbag2_py/rosbag2_py/__init__.py @@ -25,6 +25,7 @@ ) from rosbag2_py._storage import ( ConverterOptions, + FileInformation, StorageFilter, StorageOptions, TopicMetadata, @@ -55,6 +56,7 @@ __all__ = [ 'bag_rewrite', 'ConverterOptions', + 'FileInformation', 'get_registered_readers', 'get_registered_writers', 'get_registered_compressors', diff --git a/rosbag2_py/src/rosbag2_py/_reader.cpp b/rosbag2_py/src/rosbag2_py/_reader.cpp index c8f8445e06..a70a43f45c 100644 --- a/rosbag2_py/src/rosbag2_py/_reader.cpp +++ b/rosbag2_py/src/rosbag2_py/_reader.cpp @@ -45,7 +45,7 @@ class Reader void open( rosbag2_storage::StorageOptions & storage_options, - rosbag2_cpp::ConverterOptions & converter_options) + rosbag2_cpp::ConverterOptions & converter_options = rosbag2_cpp::ConverterOptions()) { reader_->open(storage_options, converter_options); } @@ -73,6 +73,11 @@ class Reader return reader_->get_all_topics_and_types(); } + const rosbag2_storage::BagMetadata & get_metadata() const + { + return reader_->get_metadata(); + } + void set_filter(const rosbag2_storage::StorageFilter & storage_filter) { return reader_->set_filter(storage_filter); @@ -108,44 +113,33 @@ std::unordered_set get_registered_readers() } // namespace rosbag2_py +using PyReader = rosbag2_py::Reader; +using PyCompressionReader = rosbag2_py::Reader; + PYBIND11_MODULE(_reader, m) { m.doc() = "Python wrapper of the rosbag2_cpp reader API"; - pybind11::class_>( - m, "SequentialReader") + pybind11::class_(m, "SequentialReader") .def(pybind11::init()) - .def("open", &rosbag2_py::Reader::open) - .def("read_next", &rosbag2_py::Reader::read_next) - .def("has_next", &rosbag2_py::Reader::has_next) - .def( - "get_all_topics_and_types", - &rosbag2_py::Reader::get_all_topics_and_types) - .def("set_filter", &rosbag2_py::Reader::set_filter) - .def("reset_filter", &rosbag2_py::Reader::reset_filter) - .def("seek", &rosbag2_py::Reader::seek); - - pybind11::class_>( - m, "SequentialCompressionReader") + .def("open", &PyReader::open) + .def("read_next", &PyReader::read_next) + .def("has_next", &PyReader::has_next) + .def("get_metadata", &PyReader::get_metadata) + .def("get_all_topics_and_types", &PyReader::get_all_topics_and_types) + .def("set_filter", &PyReader::set_filter) + .def("reset_filter", &PyReader::reset_filter) + .def("seek", &PyReader::seek); + + pybind11::class_(m, "SequentialCompressionReader") .def(pybind11::init()) - .def("open", &rosbag2_py::Reader::open) - .def( - "read_next", &rosbag2_py::Reader::read_next) - .def("has_next", &rosbag2_py::Reader::has_next) - .def( - "get_all_topics_and_types", - &rosbag2_py::Reader< - rosbag2_compression::SequentialCompressionReader - >::get_all_topics_and_types) - .def( - "set_filter", - &rosbag2_py::Reader::set_filter) - .def( - "reset_filter", - &rosbag2_py::Reader::reset_filter) - .def( - "seek", - &rosbag2_py::Reader::seek); - + .def("open", &PyCompressionReader::open) + .def("read_next", &PyCompressionReader::read_next) + .def("has_next", &PyCompressionReader::has_next) + .def("get_metadata", &PyCompressionReader::get_metadata) + .def("get_all_topics_and_types", &PyCompressionReader::get_all_topics_and_types) + .def("set_filter", &PyCompressionReader::set_filter) + .def("reset_filter", &PyCompressionReader::reset_filter) + .def("seek", &PyCompressionReader::seek); m.def( "get_registered_readers", &rosbag2_py::get_registered_readers, diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index 6979b8bda8..d9ef48282b 100644 --- a/rosbag2_py/src/rosbag2_py/_storage.cpp +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -25,14 +25,50 @@ #include "./pybind11.hpp" +namespace +{ + +using pybind11::literals::operator""_a; + +pybind11::object to_rclpy_duration(std::chrono::nanoseconds duration) +{ + pybind11::object Duration = pybind11::module::import("rclpy.duration").attr("Duration"); + return Duration("nanoseconds"_a = duration.count()); +} + +std::chrono::nanoseconds from_rclpy_duration(const pybind11::object & duration) +{ + pybind11::int_ nanos = duration.attr("nanoseconds"); + return std::chrono::nanoseconds(nanos); +} + +template +pybind11::object to_rclpy_time(T time) +{ + pybind11::object Time = pybind11::module::import("rclpy.time").attr("Time"); + return Time( + "nanoseconds"_a = std::chrono::duration_cast( + time.time_since_epoch()).count()); +} + +std::chrono::time_point from_rclpy_time( + const pybind11::object & time) +{ + pybind11::int_ nanos = time.attr("nanoseconds"); + return std::chrono::time_point( + std::chrono::nanoseconds(nanos)); +} + +} // namespace + PYBIND11_MODULE(_storage, m) { m.doc() = "Python wrapper of the rosbag2 utilities API"; pybind11::class_(m, "ConverterOptions") .def( pybind11::init(), - pybind11::arg("input_serialization_format"), - pybind11::arg("output_serialization_format")) + pybind11::arg("input_serialization_format") = "", + pybind11::arg("output_serialization_format") = "") .def_readwrite( "input_serialization_format", &rosbag2_cpp::ConverterOptions::input_serialization_format) @@ -81,9 +117,15 @@ PYBIND11_MODULE(_storage, m) { pybind11::class_(m, "StorageFilter") .def( - pybind11::init>(), - pybind11::arg("topics")) - .def_readwrite("topics", &rosbag2_storage::StorageFilter::topics); + pybind11::init, std::string, std::string>(), + pybind11::arg("topics") = std::vector(), + pybind11::arg("topics_regex") = "", + pybind11::arg("topics_regex_to_exclude") = "") + .def_readwrite("topics", &rosbag2_storage::StorageFilter::topics) + .def_readwrite("topics_regex", &rosbag2_storage::StorageFilter::topics_regex) + .def_readwrite( + "topics_regex_to_exclude", + &rosbag2_storage::StorageFilter::topics_regex_to_exclude); pybind11::class_(m, "TopicMetadata") .def( @@ -112,57 +154,118 @@ PYBIND11_MODULE(_storage, m) { pybind11::class_(m, "FileInformation") .def( - pybind11::init, - std::chrono::nanoseconds, - size_t>(), + pybind11::init( + []( + std::string path, + pybind11::object starting_time, + pybind11::object duration, + size_t message_count) + { + return rosbag2_storage::FileInformation{ + path, + from_rclpy_time(starting_time), + from_rclpy_duration(duration), + message_count + }; + }), pybind11::arg("path"), pybind11::arg("starting_time"), pybind11::arg("duration"), - pybind11::arg("message_count")) + pybind11::arg("message_count") + ) .def_readwrite("path", &rosbag2_storage::FileInformation::path) - .def_readwrite("starting_time", &rosbag2_storage::FileInformation::starting_time) + .def_property( + "starting_time", + [](const rosbag2_storage::FileInformation & self) { + return to_rclpy_time(self.starting_time); + }, + [](rosbag2_storage::FileInformation & self, const pybind11::object & value) { + self.starting_time = from_rclpy_time(value); + }) + .def_property( + "duration", + [](const rosbag2_storage::FileInformation & self) { + return to_rclpy_duration(self.duration); + }, + [](rosbag2_storage::FileInformation & self, const pybind11::object & value) { + self.duration = from_rclpy_duration(value); + }) .def_readwrite("duration", &rosbag2_storage::FileInformation::duration) .def_readwrite("message_count", &rosbag2_storage::FileInformation::message_count); pybind11::class_(m, "BagMetadata") .def( - pybind11::init< - int, - uint64_t, - std::string, - std::vector, - std::vector, - std::chrono::nanoseconds, - std::chrono::time_point, - uint64_t, - std::vector, - std::string, - std::string>(), - pybind11::arg("version"), - pybind11::arg("bag_size"), - pybind11::arg("storage_identifier"), - pybind11::arg("relative_file_paths"), - pybind11::arg("files"), - pybind11::arg("duration"), - pybind11::arg("starting_time"), - pybind11::arg("message_count"), - pybind11::arg("topics_with_message_count"), - pybind11::arg("compression_format"), - pybind11::arg("compression_mode")) + pybind11::init( + []( + int version, + uint64_t bag_size, + std::string storage_identifier, + std::vector relative_file_paths, + std::vector files, + pybind11::object duration, + pybind11::object starting_time, + uint64_t message_count, + std::vector topics_with_message_count, + std::string compression_format, + std::string compression_mode, + std::unordered_map custom_data) + { + return rosbag2_storage::BagMetadata{ + version, + bag_size, + storage_identifier, + relative_file_paths, + files, + from_rclpy_duration(duration), + from_rclpy_time(starting_time), + message_count, + topics_with_message_count, + compression_format, + compression_mode, + custom_data + }; + }), + pybind11::arg("version") = 6, + pybind11::arg("bag_size") = 0, + pybind11::arg("storage_identifier") = "", + pybind11::arg("relative_file_paths") = std::vector(), + pybind11::arg("files") = std::vector(), + pybind11::arg("duration") = to_rclpy_duration(std::chrono::nanoseconds{0}), + pybind11::arg("starting_time") = to_rclpy_time( + std::chrono::time_point(std::chrono::nanoseconds{0})), + pybind11::arg("message_count") = 0, + pybind11::arg("topics_with_message_count") = std::vector(), + pybind11::arg("compression_format") = "", + pybind11::arg("compression_mode") = "", + pybind11::arg("custom_data") = std::unordered_map()) .def_readwrite("version", &rosbag2_storage::BagMetadata::version) .def_readwrite("bag_size", &rosbag2_storage::BagMetadata::bag_size) .def_readwrite("storage_identifier", &rosbag2_storage::BagMetadata::storage_identifier) .def_readwrite("relative_file_paths", &rosbag2_storage::BagMetadata::relative_file_paths) .def_readwrite("files", &rosbag2_storage::BagMetadata::files) - .def_readwrite("duration", &rosbag2_storage::BagMetadata::duration) - .def_readwrite("starting_time", &rosbag2_storage::BagMetadata::starting_time) + .def_property( + "duration", + [](const rosbag2_storage::BagMetadata & self) { + return to_rclpy_duration(self.duration); + }, + [](rosbag2_storage::BagMetadata & self, const pybind11::object & value) { + self.duration = from_rclpy_duration(value); + }) + .def_property( + "starting_time", + [](const rosbag2_storage::BagMetadata & self) { + return to_rclpy_time(self.starting_time); + }, + [](rosbag2_storage::BagMetadata & self, const pybind11::object & value) { + self.starting_time = from_rclpy_time(value); + }) .def_readwrite("message_count", &rosbag2_storage::BagMetadata::message_count) .def_readwrite( "topics_with_message_count", &rosbag2_storage::BagMetadata::topics_with_message_count) .def_readwrite("compression_format", &rosbag2_storage::BagMetadata::compression_format) .def_readwrite("compression_mode", &rosbag2_storage::BagMetadata::compression_mode) + .def_readwrite("custom_data", &rosbag2_storage::BagMetadata::custom_data) .def( "__repr__", [](const rosbag2_storage::BagMetadata & metadata) { return format_bag_meta_data(metadata); diff --git a/rosbag2_py/test/common.py b/rosbag2_py/test/common.py index 70c0b8368a..f68210f7a8 100644 --- a/rosbag2_py/test/common.py +++ b/rosbag2_py/test/common.py @@ -12,12 +12,21 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os +import sys import time from typing import Callable -from rclpy.clock import Clock, ClockType -from rclpy.duration import Duration -import rosbag2_py +if os.environ.get('ROSBAG2_PY_TEST_WITH_RTLD_GLOBAL', None) is not None: + # This is needed on Linux when compiling with clang/libc++. + # TL;DR This makes class_loader work when using a python extension compiled with libc++. + # + # For the fun RTTI ABI details, see https://whatofhow.wordpress.com/2015/03/17/odr-rtti-dso/. + sys.setdlopenflags(os.RTLD_GLOBAL | os.RTLD_LAZY) + +from rclpy.clock import Clock, ClockType # noqa +from rclpy.duration import Duration # noqa +import rosbag2_py # noqa def get_rosbag_options(path, serialization_format='cdr'): diff --git a/rosbag2_py/test/test_convert.py b/rosbag2_py/test/test_convert.py index 03757cc51d..541247cd35 100644 --- a/rosbag2_py/test/test_convert.py +++ b/rosbag2_py/test/test_convert.py @@ -14,23 +14,14 @@ import os from pathlib import Path -import sys import tempfile import unittest -if os.environ.get('ROSBAG2_PY_TEST_WITH_RTLD_GLOBAL', None) is not None: - # This is needed on Linux when compiling with clang/libc++. - # TL;DR This makes class_loader work when using a python extension compiled with libc++. - # - # For the fun RTTI ABI details, see https://whatofhow.wordpress.com/2015/03/17/odr-rtti-dso/. - sys.setdlopenflags(os.RTLD_GLOBAL | os.RTLD_LAZY) - -from common import get_rosbag_options # noqa -import rosbag2_py # noqa +import common # noqa from rosbag2_py import ( bag_rewrite, StorageOptions, -) # noqa +) RESOURCES_PATH = Path(os.environ['ROSBAG2_PY_TEST_RESOURCES_DIR']) diff --git a/rosbag2_py/test/test_reindexer.py b/rosbag2_py/test/test_reindexer.py index 9c895f8856..463116472f 100644 --- a/rosbag2_py/test/test_reindexer.py +++ b/rosbag2_py/test/test_reindexer.py @@ -22,17 +22,9 @@ import os from pathlib import Path -import sys -if os.environ.get('ROSBAG2_PY_TEST_WITH_RTLD_GLOBAL', None) is not None: - # This is needed on Linux when compiling with clang/libc++. - # TL;DR This makes class_loader work when using a python extension compiled with libc++. - # - # For the fun RTTI ABI details, see https://whatofhow.wordpress.com/2015/03/17/odr-rtti-dso/. - sys.setdlopenflags(os.RTLD_GLOBAL | os.RTLD_LAZY) - -from common import get_rosbag_options # noqa -import rosbag2_py # noqa +from common import get_rosbag_options +import rosbag2_py RESOURCES_PATH = Path(os.environ['ROSBAG2_PY_TEST_RESOURCES_DIR']) diff --git a/rosbag2_py/test/test_sequential_reader.py b/rosbag2_py/test/test_sequential_reader.py index 3dc5d1f4b2..7a8ca2e3bf 100644 --- a/rosbag2_py/test/test_sequential_reader.py +++ b/rosbag2_py/test/test_sequential_reader.py @@ -14,22 +14,15 @@ import os from pathlib import Path -import sys + +from common import get_rosbag_options from rcl_interfaces.msg import Log from rclpy.serialization import deserialize_message +import rosbag2_py from rosidl_runtime_py.utilities import get_message from std_msgs.msg import String -if os.environ.get('ROSBAG2_PY_TEST_WITH_RTLD_GLOBAL', None) is not None: - # This is needed on Linux when compiling with clang/libc++. - # TL;DR This makes class_loader work when using a python extension compiled with libc++. - # - # For the fun RTTI ABI details, see https://whatofhow.wordpress.com/2015/03/17/odr-rtti-dso/. - sys.setdlopenflags(os.RTLD_GLOBAL | os.RTLD_LAZY) - -from common import get_rosbag_options # noqa -import rosbag2_py # noqa RESOURCES_PATH = Path(os.environ['ROSBAG2_PY_TEST_RESOURCES_DIR']) diff --git a/rosbag2_py/test/test_sequential_reader_multiple_files.py b/rosbag2_py/test/test_sequential_reader_multiple_files.py index a478878498..835ea156e2 100644 --- a/rosbag2_py/test/test_sequential_reader_multiple_files.py +++ b/rosbag2_py/test/test_sequential_reader_multiple_files.py @@ -14,18 +14,9 @@ import os from pathlib import Path -import sys - -if os.environ.get('ROSBAG2_PY_TEST_WITH_RTLD_GLOBAL', None) is not None: - # This is needed on Linux when compiling with clang/libc++. - # TL;DR This makes class_loader work when using a python extension compiled with libc++. - # - # For the fun RTTI ABI details, see https://whatofhow.wordpress.com/2015/03/17/odr-rtti-dso/. - sys.setdlopenflags(os.RTLD_GLOBAL | os.RTLD_LAZY) - -from common import get_rosbag_options # noqa -import rosbag2_py # noqa +from common import get_rosbag_options +import rosbag2_py RESOURCES_PATH = Path(os.environ['ROSBAG2_PY_TEST_RESOURCES_DIR']) diff --git a/rosbag2_py/test/test_sequential_writer.py b/rosbag2_py/test/test_sequential_writer.py index 3ad7fcc167..61012bc47e 100644 --- a/rosbag2_py/test/test_sequential_writer.py +++ b/rosbag2_py/test/test_sequential_writer.py @@ -12,23 +12,13 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os -import sys +from common import get_rosbag_options from rclpy.serialization import deserialize_message, serialize_message +import rosbag2_py from rosidl_runtime_py.utilities import get_message from std_msgs.msg import String -if os.environ.get('ROSBAG2_PY_TEST_WITH_RTLD_GLOBAL', None) is not None: - # This is needed on Linux when compiling with clang/libc++. - # TL;DR This makes class_loader work when using a python extension compiled with libc++. - # - # For the fun RTTI ABI details, see https://whatofhow.wordpress.com/2015/03/17/odr-rtti-dso/. - sys.setdlopenflags(os.RTLD_GLOBAL | os.RTLD_LAZY) - -from common import get_rosbag_options # noqa -import rosbag2_py # noqa - def create_topic(writer, topic_name, topic_type, serialization_format='cdr'): """ diff --git a/rosbag2_py/test/test_storage.py b/rosbag2_py/test/test_storage.py new file mode 100644 index 0000000000..39c6995b05 --- /dev/null +++ b/rosbag2_py/test/test_storage.py @@ -0,0 +1,111 @@ +# Copyright 2022, Foxglove Technologies. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import unittest + +import common # noqa + +from rclpy.duration import Duration +from rclpy.time import Time +from rosbag2_py import ( + BagMetadata, + ConverterOptions, + FileInformation, + StorageFilter, + StorageOptions, + TopicInformation, + TopicMetadata, +) + + +class TestStorageStructs(unittest.TestCase): + + def test_bag_metadata_default_ctor(self): + metadata = BagMetadata() + assert metadata + + def test_converter_options_ctor(self): + converter_options = ConverterOptions() + assert converter_options + + def test_file_information_ctor(self): + file_information = FileInformation( + path='test_path', + starting_time=Time(nanoseconds=1000), + duration=Duration(), + message_count=1234, + ) + assert file_information + + def test_storage_options_ctor(self): + storage_options = StorageOptions(uri='path') + assert storage_options + + def test_storage_filter_ctor(self): + storage_filter = StorageFilter() + assert storage_filter + + def test_topic_metadata_ctor(self): + topic_metadata = TopicMetadata( + name='topic', + type='msgs/Msg', + serialization_format='format' + ) + assert topic_metadata + + def test_topic_information_ctor(self): + topic_information = TopicInformation( + topic_metadata=TopicMetadata( + name='topic', + type='msgs/Msg', + serialization_format='format'), + message_count=10 + ) + assert topic_information + + def test_bag_metadata_ctor_named_args(self): + duration = Duration(nanoseconds=200) + starting_time = Time(nanoseconds=100) + + file_information = FileInformation( + path='something', + starting_time=starting_time, + duration=duration, + message_count=12) + topic_information = TopicInformation( + topic_metadata=TopicMetadata( + name='topic', + type='msgs/Msg', + serialization_format='format'), + message_count=10 + ) + + metadata = BagMetadata( + version=1, + bag_size=2, + storage_identifier='foo', + relative_file_paths=['bar', 'baz'], + files=[file_information], + duration=duration, + starting_time=starting_time, + message_count=12, + topics_with_message_count=[topic_information], + compression_format='aaaa', + compression_mode='bbbbb', + custom_data={ + 'keya': 'valuea', + 'keyb': 'valueb' + } + ) + assert metadata diff --git a/rosbag2_py/test/test_transport.py b/rosbag2_py/test/test_transport.py index 176d7e9fe2..b11612fc35 100644 --- a/rosbag2_py/test/test_transport.py +++ b/rosbag2_py/test/test_transport.py @@ -13,25 +13,15 @@ # limitations under the License. import datetime -import os from pathlib import Path -import sys import threading - from common import get_rosbag_options, wait_for import rclpy from rclpy.qos import QoSProfile import rosbag2_py from std_msgs.msg import String -if os.environ.get('ROSBAG2_PY_TEST_WITH_RTLD_GLOBAL', None) is not None: - # This is needed on Linux when compiling with clang/libc++. - # TL;DR This makes class_loader work when using a python extension compiled with libc++. - # - # For the fun RTTI ABI details, see https://whatofhow.wordpress.com/2015/03/17/odr-rtti-dso/. - sys.setdlopenflags(os.RTLD_GLOBAL | os.RTLD_LAZY) - def test_options_qos_conversion(): # Tests that the to-and-from C++ conversions are working properly in the pybind structs