diff --git a/ros2bag/ros2bag/verb/play.py b/ros2bag/ros2bag/verb/play.py index 403829b0f1..2198987e1e 100644 --- a/ros2bag/ros2bag/verb/play.py +++ b/ros2bag/ros2bag/verb/play.py @@ -96,15 +96,14 @@ def main(self, *, args): # noqa: D102 storage_id=args.storage, storage_config_uri=storage_config_file, ) - play_options = PlayOptions( - read_ahead_queue_size=args.read_ahead_queue_size, - node_prefix=NODE_NAME_PREFIX, - rate=args.rate, - topics_to_filter=args.topics, - topic_qos_profile_overrides=qos_profile_overrides, - loop=args.loop, - topic_remapping_options=topic_remapping, - ) + play_options = PlayOptions() + play_options.read_ahead_queue_size = args.read_ahead_queue_size + play_options.node_prefix = NODE_NAME_PREFIX + play_options.rate = args.rate + play_options.topics_to_filter = args.topics + play_options.topic_qos_profile_overrides = qos_profile_overrides + play_options.loop = args.loop + play_options.topic_remapping_options = topic_remapping player = Player() player.play(storage_options, play_options) diff --git a/ros2bag/ros2bag/verb/record.py b/ros2bag/ros2bag/verb/record.py index 2bc9cd9a55..69c14c0314 100644 --- a/ros2bag/ros2bag/verb/record.py +++ b/ros2bag/ros2bag/verb/record.py @@ -22,6 +22,9 @@ from ros2bag.verb import VerbExtension from ros2cli.node import NODE_NAME_PREFIX from rosbag2_py import get_registered_writers +from rosbag2_py import Recorder +from rosbag2_py import RecordOptions +from rosbag2_py import StorageOptions import yaml @@ -174,35 +177,34 @@ def main(self, *, args): # noqa: D102 if args.storage_config_file: storage_config_file = args.storage_config_file.name - # NOTE(hidmic): in merged install workspaces on Windows, Python entrypoint lookups - # combined with constrained environments (as imposed by colcon test) - # may result in DLL loading failures when attempting to import a C - # extension. Therefore, do not import rosbag2_transport at the module - # level but on demand, right before first use. - from rosbag2_transport import rosbag2_transport_py - - rosbag2_transport_py.record( + storage_options = StorageOptions( uri=uri, storage_id=args.storage, - serialization_format=args.serialization_format, - node_prefix=NODE_NAME_PREFIX, - compression_mode=args.compression_mode, - compression_format=args.compression_format, - compression_queue_size=args.compression_queue_size, - compression_threads=args.compression_threads, - all=args.all, - no_discovery=args.no_discovery, - polling_interval=args.polling_interval, max_bagfile_size=args.max_bag_size, max_bagfile_duration=args.max_bag_duration, max_cache_size=args.max_cache_size, - topics=args.topics, - regex=args.regex, - exclude=args.exclude, - include_hidden_topics=args.include_hidden_topics, - qos_profile_overrides=qos_profile_overrides, storage_preset_profile=args.storage_preset_profile, - storage_config_file=storage_config_file) + storage_config_uri=storage_config_file, + ) + record_options = RecordOptions() + record_options.all = args.all + record_options.is_discovery_disabled = args.no_discovery + record_options.topics = args.topics + record_options.rmw_serialization_format = args.serialization_format + record_options.topic_polling_interval = datetime.timedelta( + milliseconds=args.polling_interval) + record_options.regex = args.regex + record_options.exclude = args.exclude + record_options.node_prefix = NODE_NAME_PREFIX + record_options.compression_mode = args.compression_mode + record_options.compression_format = args.compression_format + record_options.compression_queue_size = args.compression_queue_size + record_options.compression_threads = args.compression_threads + record_options.topic_qos_profile_overrides = qos_profile_overrides + record_options.include_hidden_topics = args.include_hidden_topics + + recorder = Recorder() + recorder.record(storage_options, record_options) if os.path.isdir(uri) and not os.listdir(uri): os.rmdir(uri) diff --git a/ros2bag/test/test_play_qos_profiles.py b/ros2bag/test/test_play_qos_profiles.py index b6f719d48d..7d6583410c 100644 --- a/ros2bag/test/test_play_qos_profiles.py +++ b/ros2bag/test/test_play_qos_profiles.py @@ -69,7 +69,7 @@ def test_qos_simple(self): bag_command.wait_for_shutdown(timeout=5) expected_string_regex = re.compile(ERROR_STRING) matches = expected_string_regex.search(bag_command.output) - assert not matches, print('ros2bag CLI did not produce the expected output') + assert not matches, 'ros2bag CLI did not produce the expected output' def test_qos_incomplete(self): """Test a partially filled QoS profile for a single topic.""" @@ -81,4 +81,4 @@ def test_qos_incomplete(self): bag_command.wait_for_shutdown(timeout=5) expected_string_regex = re.compile(ERROR_STRING) matches = expected_string_regex.search(bag_command.output) - assert not matches, print('ros2bag CLI did not produce the expected output') + assert not matches, 'ros2bag CLI did not produce the expected output' diff --git a/ros2bag/test/test_record_qos_profiles.py b/ros2bag/test/test_record_qos_profiles.py index 91beba614f..5b4918b7e3 100644 --- a/ros2bag/test/test_record_qos_profiles.py +++ b/ros2bag/test/test_record_qos_profiles.py @@ -93,8 +93,7 @@ def test_qos_simple(self): bag_command.wait_for_shutdown(timeout=SHUTDOWN_TIMEOUT) assert bag_command.terminated matches = expected_string_regex.search(bag_command.output) - assert matches, print( - ERROR_STRING_MSG.format(expected_string_regex.pattern, bag_command.output)) + assert matches, ERROR_STRING_MSG.format(expected_string_regex.pattern, bag_command.output) def test_incomplete_qos_profile(self): profile_path = PROFILE_PATH / 'incomplete_qos_profile.yaml' @@ -110,8 +109,7 @@ def test_incomplete_qos_profile(self): bag_command.wait_for_shutdown(timeout=SHUTDOWN_TIMEOUT) assert bag_command.terminated matches = expected_string_regex.search(bag_command.output) - assert matches, print( - ERROR_STRING_MSG.format(expected_string_regex.pattern, bag_command.output)) + assert matches, ERROR_STRING_MSG.format(expected_string_regex.pattern, bag_command.output) def test_incomplete_qos_duration(self): profile_path = PROFILE_PATH / 'incomplete_qos_duration.yaml' @@ -128,8 +126,7 @@ def test_incomplete_qos_duration(self): assert bag_command.terminated assert bag_command.exit_code != launch_testing.asserts.EXIT_OK matches = expected_string_regex.search(bag_command.output) - assert matches, print( - ERROR_STRING_MSG.format(expected_string_regex.pattern, bag_command.output)) + assert matches, ERROR_STRING_MSG.format(expected_string_regex.pattern, bag_command.output) def test_nonexistent_qos_profile(self): profile_path = PROFILE_PATH / 'foobar.yaml' @@ -146,5 +143,4 @@ def test_nonexistent_qos_profile(self): assert bag_command.terminated assert bag_command.exit_code != launch_testing.asserts.EXIT_OK matches = expected_string_regex.search(bag_command.output) - assert matches, print( - ERROR_STRING_MSG.format(expected_string_regex.pattern, bag_command.output)) + assert matches, ERROR_STRING_MSG.format(expected_string_regex.pattern, bag_command.output) diff --git a/rosbag2_py/CMakeLists.txt b/rosbag2_py/CMakeLists.txt index edd5584ad6..ad1f79b3c4 100644 --- a/rosbag2_py/CMakeLists.txt +++ b/rosbag2_py/CMakeLists.txt @@ -155,6 +155,11 @@ if(BUILD_TESTING) PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}" APPEND_ENV "PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}" ${other_environment_vars} ) + ament_add_pytest_test(test_transport_py + "test/test_transport.py" + PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}" + APPEND_ENV "PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}" ${other_environment_vars} + ) endif() ament_package() diff --git a/rosbag2_py/rosbag2_py/__init__.py b/rosbag2_py/rosbag2_py/__init__.py index 782db5c074..3ef2cbc3c1 100644 --- a/rosbag2_py/rosbag2_py/__init__.py +++ b/rosbag2_py/rosbag2_py/__init__.py @@ -42,6 +42,8 @@ from rosbag2_py._transport import ( Player, PlayOptions, + Recorder, + RecordOptions, ) __all__ = [ @@ -60,4 +62,6 @@ 'Info', 'Player', 'PlayOptions', + 'Recorder', + 'RecordOptions', ] diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp index 620781369c..9a101ef21d 100644 --- a/rosbag2_py/src/rosbag2_py/_storage.cpp +++ b/rosbag2_py/src/rosbag2_py/_storage.cpp @@ -42,12 +42,14 @@ PYBIND11_MODULE(_storage, m) { pybind11::class_(m, "StorageOptions") .def( - pybind11::init(), + pybind11::init< + std::string, std::string, uint64_t, uint64_t, uint64_t, std::string, std::string>(), 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, + pybind11::arg("storage_preset_profile") = "", pybind11::arg("storage_config_uri") = "") .def_readwrite("uri", &rosbag2_storage::StorageOptions::uri) .def_readwrite("storage_id", &rosbag2_storage::StorageOptions::storage_id) @@ -60,6 +62,9 @@ PYBIND11_MODULE(_storage, m) { .def_readwrite( "max_cache_size", &rosbag2_storage::StorageOptions::max_cache_size) + .def_readwrite( + "storage_preset_profile", + &rosbag2_storage::StorageOptions::storage_preset_profile) .def_readwrite( "storage_config_uri", &rosbag2_storage::StorageOptions::storage_config_uri); diff --git a/rosbag2_py/src/rosbag2_py/_transport.cpp b/rosbag2_py/src/rosbag2_py/_transport.cpp index f74ea79565..e6c98f262b 100644 --- a/rosbag2_py/src/rosbag2_py/_transport.cpp +++ b/rosbag2_py/src/rosbag2_py/_transport.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -26,12 +27,61 @@ #include "rosbag2_cpp/writers/sequential_writer.hpp" #include "rosbag2_storage/storage_options.hpp" #include "rosbag2_transport/play_options.hpp" +#include "rosbag2_transport/record_options.hpp" #include "rosbag2_transport/rosbag2_transport.hpp" #include "./pybind11.hpp" -using PlayOptions = rosbag2_transport::PlayOptions; -using Rosbag2Transport = rosbag2_transport::Rosbag2Transport; +namespace py = pybind11; +typedef std::unordered_map QoSMap; + +namespace +{ + +rclcpp::QoS qos_from_handle(const py::handle source) +{ + auto py_capsule = PyObject_CallMethod(source.ptr(), "get_c_qos_profile", ""); + const auto rmw_qos_profile = reinterpret_cast( + PyCapsule_GetPointer(py_capsule, "rmw_qos_profile_t")); + const auto qos_init = rclcpp::QoSInitialization::from_rmw(*rmw_qos_profile); + return rclcpp::QoS{qos_init, *rmw_qos_profile}; +} + +QoSMap qos_map_from_py_dict(const py::dict & dict) +{ + QoSMap value; + for (const auto & item : dict) { + auto key = std::string(py::str(item.first)); + value.insert({key, qos_from_handle(item.second)}); + } + return value; +} + +/** + * Simple wrapper subclass to provide nontrivial type conversions for python properties. + */ +template +struct OptionsWrapper : public T +{ +public: + void setTopicQoSProfileOverrides( + const py::dict & overrides) + { + py_dict = overrides; + this->topic_qos_profile_overrides = qos_map_from_py_dict(overrides); + } + + const py::dict & getTopicQoSProfileOverrides() + { + return py_dict; + } + + py::dict py_dict; +}; +typedef OptionsWrapper PlayOptions; +typedef OptionsWrapper RecordOptions; + +} // namespace namespace rosbag2_py { @@ -44,7 +94,7 @@ class Player void play( const rosbag2_storage::StorageOptions & storage_options, - const rosbag2_transport::PlayOptions & play_options) + PlayOptions & play_options) { auto writer = std::make_shared( std::make_unique()); @@ -66,48 +116,110 @@ class Player } } - Rosbag2Transport impl(reader, writer); + rosbag2_transport::Rosbag2Transport impl(reader, writer); impl.init(); impl.play(storage_options, play_options); impl.shutdown(); } }; +class Recorder +{ +public: + Recorder() = default; + virtual ~Recorder() = default; + + void record( + const rosbag2_storage::StorageOptions & storage_options, + RecordOptions & record_options) + { + rosbag2_compression::CompressionOptions compression_options { + record_options.compression_format, + rosbag2_compression::compression_mode_from_string(record_options.compression_mode), + record_options.compression_queue_size, + record_options.compression_threads + }; + if (compression_options.compression_threads < 1) { + compression_options.compression_threads = std::thread::hardware_concurrency(); + } + + if (record_options.rmw_serialization_format.empty()) { + record_options.rmw_serialization_format = std::string(rmw_get_serialization_format()); + } + + + auto reader = std::make_shared( + std::make_unique()); + std::shared_ptr writer; + // Change writer based on recording options + if (!record_options.compression_format.empty()) { + writer = std::make_shared( + std::make_unique(compression_options)); + } else { + writer = std::make_shared( + std::make_unique()); + } + + rosbag2_transport::Rosbag2Transport impl(reader, writer); + impl.init(); + impl.record(storage_options, record_options); + impl.shutdown(); + } +}; + } // namespace rosbag2_py PYBIND11_MODULE(_transport, m) { m.doc() = "Python wrapper of the rosbag2_transport API"; - pybind11::class_(m, "PlayOptions") - .def( - pybind11::init< - size_t, - std::string, - float, - std::vector, - std::unordered_map, - bool, - std::vector - >(), - pybind11::arg("read_ahead_queue_size"), - pybind11::arg("node_prefix") = "", - pybind11::arg("rate") = 1.0, - pybind11::arg("topics_to_filter") = std::vector{}, - pybind11::arg("topic_qos_profile_overrides") = std::unordered_map{}, - pybind11::arg("loop") = false, - pybind11::arg("topic_remapping_options") = std::vector{} - ) + // NOTE: it is non-trivial to add a constructor for PlayOptions and RecordOptions + // because the rclcpp::QoS <-> rclpy.qos.QoS Profile conversion cannot be done by builtins. + // It is possible, but the code is much longer and harder to maintain, requiring duplicating + // the names of the members multiple times, as well as the default values from the struct + // definitions. + + py::class_(m, "PlayOptions") + .def(py::init<>()) .def_readwrite("read_ahead_queue_size", &PlayOptions::read_ahead_queue_size) .def_readwrite("node_prefix", &PlayOptions::node_prefix) .def_readwrite("rate", &PlayOptions::rate) .def_readwrite("topics_to_filter", &PlayOptions::topics_to_filter) - .def_readwrite("topic_qos_profile_overrides", &PlayOptions::topic_qos_profile_overrides) + .def_property( + "topic_qos_profile_overrides", + &PlayOptions::getTopicQoSProfileOverrides, + &PlayOptions::setTopicQoSProfileOverrides) .def_readwrite("loop", &PlayOptions::loop) .def_readwrite("topic_remapping_options", &PlayOptions::topic_remapping_options) ; - pybind11::class_(m, "Player") - .def(pybind11::init()) + py::class_(m, "RecordOptions") + .def(py::init<>()) + .def_readwrite("all", &RecordOptions::all) + .def_readwrite("is_discovery_disabled", &RecordOptions::is_discovery_disabled) + .def_readwrite("topics", &RecordOptions::topics) + .def_readwrite("rmw_serialization_format", &RecordOptions::rmw_serialization_format) + .def_readwrite("topic_polling_interval", &RecordOptions::topic_polling_interval) + .def_readwrite("regex", &RecordOptions::regex) + .def_readwrite("exclude", &RecordOptions::exclude) + .def_readwrite("node_prefix", &RecordOptions::node_prefix) + .def_readwrite("compression_mode", &RecordOptions::compression_mode) + .def_readwrite("compression_format", &RecordOptions::compression_format) + .def_readwrite("compression_queue_size", &RecordOptions::compression_queue_size) + .def_readwrite("compression_threads", &RecordOptions::compression_threads) + .def_property( + "topic_qos_profile_overrides", + &RecordOptions::getTopicQoSProfileOverrides, + &RecordOptions::setTopicQoSProfileOverrides) + .def_readwrite("include_hidden_topics", &RecordOptions::include_hidden_topics) + ; + + py::class_(m, "Player") + .def(py::init()) .def("play", &rosbag2_py::Player::play) ; + + py::class_(m, "Recorder") + .def(py::init()) + .def("record", &rosbag2_py::Recorder::record) + ; } diff --git a/rosbag2_py/test/test_transport.py b/rosbag2_py/test/test_transport.py new file mode 100644 index 0000000000..001d118481 --- /dev/null +++ b/rosbag2_py/test/test_transport.py @@ -0,0 +1,42 @@ +# Copyright 2021 Amazon.com, Inc. or its affiliates. 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 os +import sys + +from rclpy.qos import QoSProfile + +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) + +import rosbag2_py # noqa + + +def test_options_qos_conversion(): + # Tests that the to-and-from C++ conversions are working properly in the pybind structs + simple_overrides = { + '/topic': QoSProfile(depth=10) + } + + play_options = rosbag2_py.PlayOptions() + play_options.topic_qos_profile_overrides = simple_overrides + assert play_options.topic_qos_profile_overrides == simple_overrides + + record_options = rosbag2_py.RecordOptions() + record_options.topic_qos_profile_overrides = simple_overrides + assert record_options.topic_qos_profile_overrides == simple_overrides diff --git a/rosbag2_transport/CMakeLists.txt b/rosbag2_transport/CMakeLists.txt index f67c855c22..4717dd4db9 100644 --- a/rosbag2_transport/CMakeLists.txt +++ b/rosbag2_transport/CMakeLists.txt @@ -56,19 +56,6 @@ ament_target_dependencies(${PROJECT_NAME} yaml_cpp_vendor ) -include(cmake/configure_python.cmake) -ament_python_install_package(${PROJECT_NAME}) -add_library(rosbag2_transport_py - SHARED - src/rosbag2_transport/rosbag2_transport_python.cpp) -configure_python_c_extension_library(rosbag2_transport_py) -target_link_libraries(rosbag2_transport_py rosbag2_transport) -ament_target_dependencies(rosbag2_transport_py - rosbag2_compression - rosbag2_cpp - rosbag2_storage - rmw -) # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "ROSBAG2_TRANSPORT_BUILDING_LIBRARY") diff --git a/rosbag2_transport/cmake/configure_python.cmake b/rosbag2_transport/cmake/configure_python.cmake deleted file mode 100644 index 78e8b56668..0000000000 --- a/rosbag2_transport/cmake/configure_python.cmake +++ /dev/null @@ -1,56 +0,0 @@ -# Copyright 2018 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. - -find_package(ament_cmake_python REQUIRED) -find_package(python_cmake_module REQUIRED) -find_package(PythonExtra MODULE REQUIRED) - -set(_PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}") - -if(WIN32 AND CMAKE_BUILD_TYPE STREQUAL "Debug") - set(PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE_DEBUG}") -endif() - -function(set_properties _targetname _build_type) - - set_target_properties(${_targetname} PROPERTIES - PREFIX "" - LIBRARY_OUTPUT_DIRECTORY${_build_type} "${CMAKE_CURRENT_BINARY_DIR}/test_${PROJECT_NAME}" - RUNTIME_OUTPUT_DIRECTORY${_build_type} "${CMAKE_CURRENT_BINARY_DIR}/test_${PROJECT_NAME}" - OUTPUT_NAME "_${_targetname}${PythonExtra_EXTENSION_SUFFIX}" - SUFFIX "${PythonExtra_EXTENSION_EXTENSION}") -endfunction() - -function(configure_python_c_extension_library _library_name) - set_properties(${_library_name} "") - if(WIN32) - set_properties(${_library_name} "_DEBUG") - set_properties(${_library_name} "_MINSIZEREL") - set_properties(${_library_name} "_RELEASE") - set_properties(${_library_name} "_RELWITHDEBINFO") - endif() - - target_link_libraries(${_library_name} - ${PythonExtra_LIBRARIES} - ) - - target_include_directories(${_library_name} - PUBLIC - ${PythonExtra_INCLUDE_DIRS} - ) - - install(TARGETS ${_library_name} - DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" - ) -endfunction() diff --git a/rosbag2_transport/package.xml b/rosbag2_transport/package.xml index a99cac2a51..cc94260a03 100644 --- a/rosbag2_transport/package.xml +++ b/rosbag2_transport/package.xml @@ -12,13 +12,11 @@ ament_cmake_ros - python_cmake_module rclcpp rosbag2_compression rosbag2_cpp rosbag2_storage rmw - rpyutils shared_queues_vendor yaml_cpp_vendor diff --git a/rosbag2_transport/rosbag2_transport/__init__.py b/rosbag2_transport/rosbag2_transport/__init__.py deleted file mode 100644 index b51d784c02..0000000000 --- a/rosbag2_transport/rosbag2_transport/__init__.py +++ /dev/null @@ -1,17 +0,0 @@ -# Copyright 2018 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 import_c_library - -rosbag2_transport_py = import_c_library('._rosbag2_transport_py', package='rosbag2_transport') diff --git a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp b/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp deleted file mode 100644 index b841b2926d..0000000000 --- a/rosbag2_transport/src/rosbag2_transport/rosbag2_transport_python.cpp +++ /dev/null @@ -1,269 +0,0 @@ -// Copyright 2018 Open Source Robotics Foundation, Inc. -// Copyright 2020, TNG Technology Consulting GmbH. -// -// 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 -#include - -#include "rclcpp/qos.hpp" - -#include "rosbag2_compression/compression_options.hpp" -#include "rosbag2_compression/sequential_compression_reader.hpp" -#include "rosbag2_compression/sequential_compression_writer.hpp" -#include "rosbag2_cpp/reader.hpp" -#include "rosbag2_cpp/readers/sequential_reader.hpp" -#include "rosbag2_cpp/writer.hpp" -#include "rosbag2_cpp/writers/sequential_writer.hpp" -#include "rosbag2_storage/metadata_io.hpp" -#include "rosbag2_storage/storage_options.hpp" -#include "rosbag2_transport/rosbag2_transport.hpp" -#include "rosbag2_transport/record_options.hpp" -#include "rmw/rmw.h" - -namespace -{ -/// Convert a Python3 unicode string to a native C++ std::string -std::string PyObject_AsStdString(PyObject * object) -{ - PyObject * python_string = nullptr; - if (PyUnicode_Check(object)) { - python_string = PyUnicode_AsUTF8String(object); - } else { - throw std::runtime_error("Unable to decode Python string to std::string."); - } - return std::string(PyBytes_AsString(python_string)); -} - -/// Get the rmw_qos_profile_t pointer from the rclpy QoSProfile -rmw_qos_profile_t * PyQoSProfile_AsRmwQoSProfile(PyObject * object) -{ - auto py_capsule = PyObject_CallMethod(object, "get_c_qos_profile", ""); - return reinterpret_cast( - PyCapsule_GetPointer(py_capsule, "rmw_qos_profile_t")); -} - -std::unordered_map PyObject_AsTopicQoSMap(PyObject * object) -{ - std::unordered_map topic_qos_overrides{}; - if (PyDict_Check(object)) { - PyObject * key{nullptr}; - PyObject * value{nullptr}; - Py_ssize_t pos{0}; - while (PyDict_Next(object, &pos, &key, &value)) { - auto topic_name = PyObject_AsStdString(key); - auto rmw_qos_profile = PyQoSProfile_AsRmwQoSProfile(value); - auto qos_init = rclcpp::QoSInitialization::from_rmw(*rmw_qos_profile); - auto qos_profile = rclcpp::QoS(qos_init, *rmw_qos_profile); - topic_qos_overrides.insert({topic_name, qos_profile}); - } - } else { - throw std::runtime_error{"QoS profile overrides object is not a Python dictionary."}; - } - return topic_qos_overrides; -} - -} // namespace - -static PyObject * -rosbag2_transport_record(PyObject * Py_UNUSED(self), PyObject * args, PyObject * kwargs) -{ - rosbag2_storage::StorageOptions storage_options{}; - rosbag2_transport::RecordOptions record_options{}; - - static const char * kwlist[] = { - "uri", - "storage_id", - "serialization_format", - "node_prefix", - "compression_mode", - "compression_format", - "compression_queue_size", - "compression_threads", - "all", - "no_discovery", - "polling_interval", - "max_bagfile_size", - "max_bagfile_duration", - "max_cache_size", - "topics", - "regex", - "exclude", - "include_hidden_topics", - "qos_profile_overrides", - "storage_preset_profile", - "storage_config_file", - nullptr}; - - char * uri = nullptr; - char * storage_id = nullptr; - char * serilization_format = nullptr; - char * node_prefix = nullptr; - char * compression_mode = nullptr; - char * compression_format = nullptr; - uint64_t compression_queue_size = 1; - uint64_t compression_threads = 0; - PyObject * qos_profile_overrides = nullptr; - bool all = false; - bool no_discovery = false; - uint64_t polling_interval_ms = 100; - unsigned long long max_bagfile_size = 0; // NOLINT - unsigned long long max_bagfile_duration = 0; // NOLINT - uint64_t max_cache_size = 0u; - PyObject * topics = nullptr; - char * regex = nullptr; - char * exclude = nullptr; - bool include_hidden_topics = false; - char * storage_preset_profile = nullptr; - char * storage_config_file = nullptr; - if ( - !PyArg_ParseTupleAndKeywords( - args, kwargs, "ssssss|KKbbKKKKOssbOss", const_cast(kwlist), - &uri, - &storage_id, - &serilization_format, - &node_prefix, - &compression_mode, - &compression_format, - &compression_queue_size, - &compression_threads, - &all, - &no_discovery, - &polling_interval_ms, - &max_bagfile_size, - &max_bagfile_duration, - &max_cache_size, - &topics, - ®ex, - &exclude, - &include_hidden_topics, - &qos_profile_overrides, - &storage_preset_profile, - &storage_config_file - )) - { - return nullptr; - } - - storage_options.uri = std::string(uri); - storage_options.storage_id = std::string(storage_id); - storage_options.storage_config_uri = std::string(storage_config_file); - storage_options.max_bagfile_size = (uint64_t) max_bagfile_size; - storage_options.max_bagfile_duration = static_cast(max_bagfile_duration); - storage_options.max_cache_size = max_cache_size; - storage_options.storage_preset_profile = storage_preset_profile; - record_options.all = all; - record_options.regex = regex; - record_options.exclude = exclude; - record_options.is_discovery_disabled = no_discovery; - record_options.topic_polling_interval = std::chrono::milliseconds(polling_interval_ms); - record_options.node_prefix = std::string(node_prefix); - record_options.compression_mode = std::string(compression_mode); - record_options.compression_format = compression_format; - record_options.compression_queue_size = compression_queue_size; - if (compression_threads < 1) { - compression_threads = std::thread::hardware_concurrency(); - } - record_options.compression_threads = compression_threads; - record_options.include_hidden_topics = include_hidden_topics; - - rosbag2_compression::CompressionOptions compression_options{ - record_options.compression_format, - rosbag2_compression::compression_mode_from_string(record_options.compression_mode), - record_options.compression_queue_size, - record_options.compression_threads - }; - - auto topic_qos_overrides = PyObject_AsTopicQoSMap(qos_profile_overrides); - record_options.topic_qos_profile_overrides = topic_qos_overrides; - - if (topics) { - PyObject * topic_iterator = PyObject_GetIter(topics); - if (topic_iterator != nullptr) { - PyObject * topic; - while ((topic = PyIter_Next(topic_iterator))) { - record_options.topics.emplace_back(PyUnicode_AsUTF8(topic)); - - Py_DECREF(topic); - } - Py_DECREF(topic_iterator); - } - } - record_options.rmw_serialization_format = std::string(serilization_format).empty() ? - rmw_get_serialization_format() : - serilization_format; - - // Specify defaults - auto reader = std::make_shared( - std::make_unique()); - std::shared_ptr writer; - // Change writer based on recording options - if (record_options.compression_format == "zstd") { - writer = std::make_shared( - std::make_unique(compression_options)); - } else { - writer = std::make_shared( - std::make_unique()); - } - - rosbag2_transport::Rosbag2Transport transport(reader, writer); - transport.init(); - transport.record(storage_options, record_options); - transport.shutdown(); - - Py_RETURN_NONE; -} - -/// Define the public methods of this module -#if __GNUC__ >= 8 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wcast-function-type" -#endif -static PyMethodDef rosbag2_transport_methods[] = { - { - "record", reinterpret_cast(rosbag2_transport_record), METH_VARARGS | METH_KEYWORDS, - "Record to bag" - }, - {nullptr, nullptr, 0, nullptr} /* sentinel */ -}; -#if __GNUC__ >= 8 -# pragma GCC diagnostic pop -#endif - -PyDoc_STRVAR( - rosbag2_transport__doc__, - "Python module for rosbag2 transport"); - -/// Define the Python module -static struct PyModuleDef _rosbag2_transport_module = { - PyModuleDef_HEAD_INIT, - "_rosbag2_transport", - rosbag2_transport__doc__, - -1, /* -1 means that the module keeps state in global variables */ - rosbag2_transport_methods, - nullptr, - nullptr, - nullptr, - nullptr -}; - -/// Init function of this module -PyMODINIT_FUNC PyInit__rosbag2_transport_py(void) -{ - return PyModule_Create(&_rosbag2_transport_module); -}