Added convert_rosbag_qos_to_rlcpy_qos function

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
ros2 · May 16, 2024 · ebca8d1 · ebca8d1
1 parent 5a06430
commit ebca8d1
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 0 deletions.
diff --git a/rosbag2_py/rosbag2_py/__init__.py b/rosbag2_py/rosbag2_py/__init__.py
@@ -41,6 +41,7 @@
         StorageOptions,
         TopicMetadata,
         TopicInformation,
+        convert_rosbag_qos_to_rlcpy_qos,
         get_default_storage_id,
         to_rclcpp_qos_vector,
     )
@@ -68,6 +69,7 @@
 
 __all__ = [
     'bag_rewrite',
+    'convert_rosbag_qos_to_rlcpy_qos',
     'CompressionMode',
     'CompressionOptions',
     'compression_mode_from_string',

diff --git a/rosbag2_py/rosbag2_py/_storage.pyi b/rosbag2_py/rosbag2_py/_storage.pyi
@@ -1,6 +1,7 @@
 from typing import ClassVar, Dict, List
 
 import datetime
+from rclpy import QoSProfile
 
 class BagMetadata:
     bag_size: int
@@ -214,3 +215,4 @@ class rmw_qos_reliability_policy_t:
 
 def get_default_storage_id() -> str: ...
 def to_rclcpp_qos_vector(arg0: str, arg1: int) -> List[QoS]: ...
+def convert_rosbag_qos_to_rlcpy_qos(arg0: QoS) -> List[QoSProfile]: ...
diff --git a/rosbag2_py/src/rosbag2_py/_storage.cpp b/rosbag2_py/src/rosbag2_py/_storage.cpp
@@ -391,6 +391,42 @@ PYBIND11_MODULE(_storage, m) {
     &rosbag2_storage::to_rclcpp_qos_vector,
     "Convert QoS in YAML to std::vector<QoS>");
 
+  m.def(
+    "convert_rosbag_qos_to_rlcpy_qos",
+    [](rclcpp::QoS qos_input) {
+      pybind11::object rclcpy_qos = pybind11::module_::import("rclpy.qos");
+      pybind11::object rclcpy_duration = pybind11::module_::import("rclpy.duration");
+      pybind11::object duration_lifespan = rclcpy_qos.attr("Duration")(
+        "seconds"_a=qos_input.lifespan().to_rmw_time().sec,
+        "nanoseconds"_a=qos_input.lifespan().to_rmw_time().nsec);
+      pybind11::object duration_deadline = rclcpy_qos.attr("Duration")(
+        "seconds"_a=qos_input.deadline().to_rmw_time().sec,
+        "nanoseconds"_a=qos_input.deadline().to_rmw_time().nsec);
+      pybind11::object duration_liveliness_lease_duration = rclcpy_qos.attr("Duration")(
+        "seconds"_a=qos_input.liveliness_lease_duration().to_rmw_time().sec,
+        "nanoseconds"_a=qos_input.liveliness_lease_duration().to_rmw_time().nsec);
+
+      auto history = rclcpp::HistoryPolicy::KeepLast;
+      if (qos_input.history() != rclcpp::HistoryPolicy::Unknown) {
+         history = qos_input.history();
+      }
+
+      using namespace pybind11::literals;
+      pybind11::object qos_profile = rclcpy_qos.attr("QoSProfile")(
+        "depth"_a=qos_input.depth(),
+        "history"_a=static_cast<int>(history),
+        "reliability"_a=static_cast<int>(qos_input.reliability()),
+        "durability"_a=static_cast<int>(qos_input.durability()),
+        "lifespan"_a=duration_lifespan,
+        "deadline"_a=duration_deadline,
+        "liveliness"_a=static_cast<int>(qos_input.liveliness()),
+        "liveliness_lease_duration"_a=duration_liveliness_lease_duration,
+        "avoid_ros_namespace_conventions"_a=static_cast<bool>(qos_input.avoid_ros_namespace_conventions()));
+
+      return qos_profile;
+    },
+    "Convert QoS in YAML to std::vector<QoS>");
+
   pybind11::class_<rosbag2_storage::MetadataIo>(m, "MetadataIo")
   .def(pybind11::init<>())
   .def("write_metadata", &rosbag2_storage::MetadataIo::write_metadata)

diff --git a/rosbag2_py/src/rosbag2_py/pybind11.hpp b/rosbag2_py/src/rosbag2_py/pybind11.hpp
@@ -27,5 +27,6 @@
 #endif
 #include <pybind11/stl.h>
 #include <pybind11/chrono.h>
+#include <pybind11/embed.h>
 
 #endif  // ROSBAG2_PY__PYBIND11_HPP_