diff --git a/rosbag2_py/AMENT_IGNORE b/rosbag2_py/AMENT_IGNORE new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rosbag2_py/CMakeLists.txt b/rosbag2_py/CMakeLists.txt new file mode 100644 index 0000000000..98b7fd6385 --- /dev/null +++ b/rosbag2_py/CMakeLists.txt @@ -0,0 +1,88 @@ +cmake_minimum_required(VERSION 3.5) +project(rosbag2_py) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(pybind11 REQUIRED) +find_package(python_cmake_module REQUIRED) +find_package(rosbag2_compression REQUIRED) +find_package(rosbag2_cpp REQUIRED) +find_package(rosbag2_storage REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +pybind11_add_module(_reader SHARED + src/rosbag2_py/_reader.cpp +) +ament_target_dependencies(_reader PUBLIC + "rosbag2_compression" + "rosbag2_cpp" +) + +pybind11_add_module(_storage SHARED + src/rosbag2_py/_storage.cpp +) +ament_target_dependencies(_storage PUBLIC + "rosbag2_cpp" + "rosbag2_storage" +) + +pybind11_add_module(_writer SHARED + src/rosbag2_py/_writer.cpp +) +ament_target_dependencies(_writer PUBLIC + "rosbag2_compression" + "rosbag2_cpp" +) + +# Install cython modules as sub-modules of the project +install( + TARGETS + _reader + _storage + _writer + DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + find_package(ament_cmake_pytest REQUIRED) + + set(_PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}") + if(WIN32 AND "${CMAKE_BUILD_TYPE}" STREQUAL "Debug") + set(_PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE_DEBUG}") + endif() + set(pythonpath "${CMAKE_CURRENT_BINARY_DIR}/rosbag2_py;${CMAKE_CURRENT_SOURCE_DIR}") + if(NOT WIN32) + string(REPLACE ";" ":" pythonpath "${pythonpath}") + endif() + ament_add_pytest_test(test_sequential_reader_py "test/test_sequential_reader.py" + PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}" + APPEND_ENV "PYTHONPATH=${pythonpath}" + APPEND_LIBRARY_DIRS "${_append_library_dirs}" + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/resources" + ) + ament_add_pytest_test(test_sequential_writer_py "test/test_sequential_writer.py" + PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}" + APPEND_ENV "PYTHONPATH=${pythonpath}" + APPEND_LIBRARY_DIRS "${_append_library_dirs}" + WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/resources" + ) +endif() + +ament_package() diff --git a/rosbag2_py/package.xml b/rosbag2_py/package.xml new file mode 100644 index 0000000000..3f940523ac --- /dev/null +++ b/rosbag2_py/package.xml @@ -0,0 +1,38 @@ + + + + rosbag2_py + 0.2.4 + Python API for rosbag2 + Jacob Perron + ROS Tooling Working Group + Apache License 2.0 + + Mabel Zhang + + ament_cmake_ros + + pybind11_vendor + python_cmake_module + rosbag2_compression + rosbag2_cpp + rosbag2_storage + + rpyutils + + ament_lint_auto + ament_lint_common + python3-pytest + rcl_interfaces + rclpy + rosbag2_converter_default_plugins + rosbag2_storage_default_plugins + rosbag2_storage + rosidl_runtime_py + rpyutils + std_msgs + + + ament_cmake + + diff --git a/rosbag2_py/pytest.ini b/rosbag2_py/pytest.ini new file mode 100644 index 0000000000..fe55d2ed64 --- /dev/null +++ b/rosbag2_py/pytest.ini @@ -0,0 +1,2 @@ +[pytest] +junit_family=xunit2 diff --git a/rosbag2_py/resources/talker/metadata.yaml b/rosbag2_py/resources/talker/metadata.yaml new file mode 100644 index 0000000000..35db28401c --- /dev/null +++ b/rosbag2_py/resources/talker/metadata.yaml @@ -0,0 +1,31 @@ +rosbag2_bagfile_information: + version: 4 + storage_identifier: sqlite3 + relative_file_paths: + - talker.db3 + duration: + nanoseconds: 4531096768 + starting_time: + nanoseconds_since_epoch: 1585866235112411371 + message_count: 20 + topics_with_message_count: + - topic_metadata: + name: /topic + type: std_msgs/msg/String + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 2147483647\n nsec: 4294967295\n lifespan:\n sec: 2147483647\n nsec: 4294967295\n liveliness: 1\n liveliness_lease_duration:\n sec: 2147483647\n nsec: 4294967295\n avoid_ros_namespace_conventions: false" + message_count: 10 + - topic_metadata: + name: /rosout + type: rcl_interfaces/msg/Log + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 1\n deadline:\n sec: 2147483647\n nsec: 4294967295\n lifespan:\n sec: 10\n nsec: 0\n liveliness: 1\n liveliness_lease_duration:\n sec: 2147483647\n nsec: 4294967295\n avoid_ros_namespace_conventions: false" + message_count: 10 + - topic_metadata: + name: /parameter_events + type: rcl_interfaces/msg/ParameterEvent + serialization_format: cdr + offered_qos_profiles: "- history: 3\n depth: 0\n reliability: 1\n durability: 2\n deadline:\n sec: 2147483647\n nsec: 4294967295\n lifespan:\n sec: 2147483647\n nsec: 4294967295\n liveliness: 1\n liveliness_lease_duration:\n sec: 2147483647\n nsec: 4294967295\n avoid_ros_namespace_conventions: false" + message_count: 0 + compression_format: "" + compression_mode: "" diff --git a/rosbag2_py/resources/talker/talker.db3 b/rosbag2_py/resources/talker/talker.db3 new file mode 100644 index 0000000000..3af2110945 Binary files /dev/null and b/rosbag2_py/resources/talker/talker.db3 differ diff --git a/rosbag2_py/resources/talker/talker.db3-shm b/rosbag2_py/resources/talker/talker.db3-shm new file mode 100644 index 0000000000..fe9ac2845e Binary files /dev/null and b/rosbag2_py/resources/talker/talker.db3-shm differ diff --git a/rosbag2_py/resources/talker/talker.db3-wal b/rosbag2_py/resources/talker/talker.db3-wal new file mode 100644 index 0000000000..e69de29bb2 diff --git a/rosbag2_py/rosbag2_py/__init__.py b/rosbag2_py/rosbag2_py/__init__.py new file mode 100644 index 0000000000..7c52783d6b --- /dev/null +++ b/rosbag2_py/rosbag2_py/__init__.py @@ -0,0 +1,42 @@ +# Copyright 2020 Open Source Robotics Foundation, Inc. +# +# 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. + +from rpyutils import add_dll_directories_from_env + +# Since Python 3.8, on Windows we should ensure DLL directories are explicitly added +# to the search path. +# See https://docs.python.org/3/whatsnew/3.8.html#bpo-36085-whatsnew +with add_dll_directories_from_env('PATH'): + from rosbag2_py._reader import \ + SequentialCompressionReader, \ + SequentialReader + from rosbag2_py._storage import \ + ConverterOptions, \ + StorageFilter, \ + StorageOptions, \ + TopicMetadata + from rosbag2_py._writer import \ + SequentialCompressionWriter, \ + SequentialWriter + +__all__ = [ + 'ConverterOptions', + 'SequentialCompressionReader', + 'SequentialCompressionWriter', + 'SequentialReader', + 'SequentialWriter', + 'StorageFilter', + 'StorageOptions', + 'TopicMetadata', +] diff --git a/rosbag2_py/src/rosbag2_py/_reader.cpp b/rosbag2_py/src/rosbag2_py/_reader.cpp new file mode 100644 index 0000000000..a14a1e1694 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/_reader.cpp @@ -0,0 +1,120 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// 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. + +#include +#include + +#include +#include +#include + +#include "rosbag2_compression/sequential_compression_reader.hpp" +#include "rosbag2_cpp/converter_options.hpp" +#include "rosbag2_cpp/readers/sequential_reader.hpp" +#include "rosbag2_cpp/reader.hpp" +#include "rosbag2_cpp/storage_options.hpp" +#include "rosbag2_storage/storage_filter.hpp" +#include "rosbag2_storage/topic_metadata.hpp" + +namespace rosbag2_py +{ + +template +class Reader +{ +public: + Reader() + : reader_(std::make_unique(std::make_unique())) + { + } + + void open( + rosbag2_cpp::StorageOptions & storage_options, + rosbag2_cpp::ConverterOptions & converter_options) + { + reader_->open(storage_options, converter_options); + } + + bool has_next() + { + return reader_->has_next(); + } + + /// Return a tuple containing the topic name, the serialized ROS message, and + /// the timestamp. + pybind11::tuple read_next() + { + const auto next = reader_->read_next(); + rcutils_uint8_array_t rcutils_data = *next->serialized_data.get(); + std::string serialized_data(rcutils_data.buffer, + rcutils_data.buffer + rcutils_data.buffer_length); + return pybind11::make_tuple( + next->topic_name, pybind11::bytes(serialized_data), next->time_stamp); + } + + /// Return a mapping from topic name to topic type. + std::vector get_all_topics_and_types() + { + return reader_->get_all_topics_and_types(); + } + + void set_filter(const rosbag2_storage::StorageFilter & storage_filter) + { + return reader_->set_filter(storage_filter); + } + + void reset_filter() + { + reader_->reset_filter(); + } + +protected: + std::unique_ptr reader_; +}; +} // namespace rosbag2_py + +PYBIND11_MODULE(_reader, m) { + m.doc() = "Python wrapper of the rosbag2_cpp reader API"; + + 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); + + 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); +} diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp new file mode 100644 index 0000000000..bbd536dc51 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -0,0 +1,83 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// 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. + +#include +#include + +#include +#include + +#include "rosbag2_cpp/converter_options.hpp" +#include "rosbag2_cpp/storage_options.hpp" +#include "rosbag2_storage/storage_filter.hpp" +#include "rosbag2_storage/topic_metadata.hpp" + +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")) + .def_readwrite( + "input_serialization_format", + &rosbag2_cpp::ConverterOptions::input_serialization_format) + .def_readwrite( + "output_serialization_format", + &rosbag2_cpp::ConverterOptions::output_serialization_format); + + pybind11::class_(m, "StorageOptions") + .def( + pybind11::init(), + pybind11::arg("uri"), + pybind11::arg("storage_id"), + pybind11::arg("max_bagfile_size") = 0, + pybind11::arg("max_bagfile_duration") = 0, + pybind11::arg("max_cache_size") = 0) + .def_readwrite("uri", &rosbag2_cpp::StorageOptions::uri) + .def_readwrite("storage_id", &rosbag2_cpp::StorageOptions::storage_id) + .def_readwrite( + "max_bagfile_size", + &rosbag2_cpp::StorageOptions::max_bagfile_size) + .def_readwrite( + "max_bagfile_duration", + &rosbag2_cpp::StorageOptions::max_bagfile_duration) + .def_readwrite( + "max_cache_size", + &rosbag2_cpp::StorageOptions::max_cache_size); + + pybind11::class_(m, "StorageFilter") + .def( + pybind11::init>(), + pybind11::arg("topics")) + .def_readwrite("topics", &rosbag2_storage::StorageFilter::topics); + + pybind11::class_(m, "TopicMetadata") + .def( + pybind11::init(), + pybind11::arg("name"), + pybind11::arg("type"), + pybind11::arg("serialization_format"), + pybind11::arg("offered_qos_profiles") = "") + .def_readwrite("name", &rosbag2_storage::TopicMetadata::name) + .def_readwrite("type", &rosbag2_storage::TopicMetadata::type) + .def_readwrite( + "serialization_format", + &rosbag2_storage::TopicMetadata::serialization_format) + .def_readwrite( + "offered_qos_profiles", + &rosbag2_storage::TopicMetadata::offered_qos_profiles) + .def("equals", &rosbag2_storage::TopicMetadata::operator==); +} diff --git a/rosbag2_py/src/rosbag2_py/_writer.cpp b/rosbag2_py/src/rosbag2_py/_writer.cpp new file mode 100644 index 0000000000..ecae08a344 --- /dev/null +++ b/rosbag2_py/src/rosbag2_py/_writer.cpp @@ -0,0 +1,103 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// 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. + +#include +#include + +#include +#include + +#include "rosbag2_compression/sequential_compression_writer.hpp" +#include "rosbag2_cpp/converter_options.hpp" +#include "rosbag2_cpp/storage_options.hpp" +#include "rosbag2_cpp/writer.hpp" +#include "rosbag2_cpp/writers/sequential_writer.hpp" +#include "rosbag2_storage/ros_helper.hpp" +#include "rosbag2_storage/storage_filter.hpp" +#include "rosbag2_storage/topic_metadata.hpp" + +namespace rosbag2_py +{ + +template +class Writer +{ +public: + Writer() + : writer_(std::make_unique(std::make_unique())) + { + } + + void open( + rosbag2_cpp::StorageOptions & storage_options, + rosbag2_cpp::ConverterOptions & converter_options) + { + writer_->open(storage_options, converter_options); + } + + void create_topic(const rosbag2_storage::TopicMetadata & topic_with_type) + { + writer_->create_topic(topic_with_type); + } + + void remove_topic(const rosbag2_storage::TopicMetadata & topic_with_type) + { + writer_->remove_topic(topic_with_type); + } + + /// Write a serialized message to a bag file + void write( + const std::string & topic_name, const std::string & message, + const rcutils_time_point_value_t & time_stamp) + { + auto bag_message = + std::make_shared(); + + bag_message->topic_name = topic_name; + bag_message->serialized_data = + rosbag2_storage::make_serialized_message(message.c_str(), message.length()); + bag_message->time_stamp = time_stamp; + + writer_->write(bag_message); + } + +protected: + std::unique_ptr writer_; +}; + +} // namespace rosbag2_py + +PYBIND11_MODULE(_writer, m) { + m.doc() = "Python wrapper of the rosbag2_cpp writer API"; + + pybind11::class_>( + m, "SequentialWriter") + .def(pybind11::init()) + .def("open", &rosbag2_py::Writer::open) + .def("write", &rosbag2_py::Writer::write) + .def("remove_topic", &rosbag2_py::Writer::remove_topic) + .def("create_topic", &rosbag2_py::Writer::create_topic); + + pybind11::class_>( + m, "SequentialCompressionWriter") + .def(pybind11::init()) + .def("open", &rosbag2_py::Writer::open) + .def("write", &rosbag2_py::Writer::write) + .def( + "remove_topic", + &rosbag2_py::Writer::remove_topic) + .def( + "create_topic", + &rosbag2_py::Writer::create_topic); +} diff --git a/rosbag2_py/test/common.py b/rosbag2_py/test/common.py new file mode 100644 index 0000000000..cec02bea31 --- /dev/null +++ b/rosbag2_py/test/common.py @@ -0,0 +1,25 @@ +# Copyright 2020 Open Source Robotics Foundation, Inc. +# +# 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 rosbag2_py + + +def get_rosbag_options(path, serialization_format='cdr'): + storage_options = rosbag2_py.StorageOptions(uri=path, storage_id='sqlite3') + + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format=serialization_format, + output_serialization_format=serialization_format) + + return storage_options, converter_options diff --git a/rosbag2_py/test/test_sequential_reader.py b/rosbag2_py/test/test_sequential_reader.py new file mode 100644 index 0000000000..fc2be12c2b --- /dev/null +++ b/rosbag2_py/test/test_sequential_reader.py @@ -0,0 +1,70 @@ +# Copyright 2020 Open Source Robotics Foundation, Inc. +# +# 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. + +from pathlib import Path + +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 + + +def test_sequential_reader(): + bag_path = str(Path(__file__).parent.parent / 'resources' / 'talker') + storage_options, converter_options = get_rosbag_options(bag_path) + + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + + topic_types = reader.get_all_topics_and_types() + + # Create a map for quicker lookup + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + + # Set filter for topic of string type + storage_filter = rosbag2_py.StorageFilter(topics=['/topic']) + reader.set_filter(storage_filter) + + msg_counter = 0 + + while reader.has_next(): + (topic, data, t) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + + assert isinstance(msg, String) + assert msg.data == f'Hello, world! {msg_counter}' + + msg_counter += 1 + + # No filter + reader.reset_filter() + + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + + msg_counter = 0 + + while reader.has_next(): + (topic, data, t) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + + assert isinstance(msg, Log) or isinstance(msg, String) + + if isinstance(msg, String): + assert msg.data == f'Hello, world! {msg_counter}' + msg_counter += 1 diff --git a/rosbag2_py/test/test_sequential_writer.py b/rosbag2_py/test/test_sequential_writer.py new file mode 100644 index 0000000000..e1a12f4e53 --- /dev/null +++ b/rosbag2_py/test/test_sequential_writer.py @@ -0,0 +1,85 @@ +# Copyright 2020 Open Source Robotics Foundation, Inc. +# +# 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. + +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 + + +def create_topic(writer, topic_name, topic_type, serialization_format='cdr'): + """ + Create a new topic. + + :param writer: writer instance + :param topic_name: + :param topic_type: + :param serialization_format: + :return: + """ + topic_name = topic_name + topic = rosbag2_py.TopicMetadata(name=topic_name, type=topic_type, + serialization_format=serialization_format) + + writer.create_topic(topic) + + +def test_sequential_writer(tmp_path): + """ + Test for sequential writer. + + :return: + """ + bag_path = str(tmp_path / 'tmp_write_test') + + storage_options, converter_options = get_rosbag_options(bag_path) + + writer = rosbag2_py.SequentialWriter() + writer.open(storage_options, converter_options) + + # create topic + topic_name = '/chatter' + create_topic(writer, topic_name, 'std_msgs/msg/String') + + for i in range(10): + msg = String() + msg.data = f'Hello, world! {str(i)}' + time_stamp = i * 100 + + writer.write(topic_name, serialize_message(msg), time_stamp) + + # close bag and create new storage instance + del writer + storage_options, converter_options = get_rosbag_options(bag_path) + + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + + topic_types = reader.get_all_topics_and_types() + + # Create a map for quicker lookup + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + + msg_counter = 0 + while reader.has_next(): + topic, data, t = reader.read_next() + msg_type = get_message(type_map[topic]) + msg_deserialized = deserialize_message(data, msg_type) + + assert isinstance(msg_deserialized, String) + assert msg_deserialized.data == f'Hello, world! {msg_counter}' + assert t == msg_counter * 100 + + msg_counter += 1