From ebca8d12395eb831339f89ffadd69f04e4b8de2f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= <ahcorde@gmail.com>
Date: Thu, 16 May 2024 16:10:21 +0200
Subject: [PATCH] Added convert_rosbag_qos_to_rlcpy_qos function
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
---
 rosbag2_py/rosbag2_py/__init__.py      |  2 ++
 rosbag2_py/rosbag2_py/_storage.pyi     |  2 ++
 rosbag2_py/src/rosbag2_py/_storage.cpp | 36 ++++++++++++++++++++++++++
 rosbag2_py/src/rosbag2_py/pybind11.hpp |  1 +
 4 files changed, 41 insertions(+)

diff --git a/rosbag2_py/rosbag2_py/__init__.py b/rosbag2_py/rosbag2_py/__init__.py
index 6f084da507..adced46705 100644
--- 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
index abf846c7ce..e839c12308 100644
--- 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
index 4fc0999308..07eacc7952 100644
--- 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
index fd6fa9af16..fd25c34ce9 100644
--- 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_