From f5fac609082dec37335ebf59fbc1812ffece20fd Mon Sep 17 00:00:00 2001
From: ivanpauno <ivanpauno@ekumenlabs.com>
Date: Mon, 25 Mar 2019 18:39:27 -0300
Subject: [PATCH] Added get_actual_qos method to publisher.

Signed-off-by: ivanpauno <ivanpauno@ekumenlabs.com>
---
 rclcpp/include/rclcpp/publisher.hpp |  6 ++++++
 rclcpp/src/rclcpp/publisher.cpp     | 26 +++++++++++++++++++++++++-
 2 files changed, 31 insertions(+), 1 deletion(-)

diff --git a/rclcpp/include/rclcpp/publisher.hpp b/rclcpp/include/rclcpp/publisher.hpp
index 4500fedab5..5e05fefad4 100644
--- a/rclcpp/include/rclcpp/publisher.hpp
+++ b/rclcpp/include/rclcpp/publisher.hpp
@@ -128,6 +128,12 @@ class PublisherBase
   size_t
   get_intra_process_subscription_count() const;
 
+  /// Get actual qos settings
+  /** \return The number of subscriptions. */
+  RCLCPP_PUBLIC
+  std::shared_ptr<rmw_qos_profile_t>
+  get_actual_qos() const;
+
   /// Compare this publisher to a gid.
   /**
    * Note that this function calls the next function.
diff --git a/rclcpp/src/rclcpp/publisher.cpp b/rclcpp/src/rclcpp/publisher.cpp
index ce6ebadaee..fefd529549 100644
--- a/rclcpp/src/rclcpp/publisher.cpp
+++ b/rclcpp/src/rclcpp/publisher.cpp
@@ -159,7 +159,7 @@ PublisherBase::get_subscription_count() const
 {
   size_t inter_process_subscription_count = 0;
 
-  rmw_ret_t status = rcl_publisher_get_subscription_count(
+  rcl_ret_t status = rcl_publisher_get_subscription_count(
     &publisher_handle_,
     &inter_process_subscription_count);
 
@@ -197,6 +197,30 @@ PublisherBase::get_intra_process_subscription_count() const
   return ipm->get_subscription_count(intra_process_publisher_id_);
 }
 
+std::shared_ptr<rmw_qos_profile_t>
+PublisherBase::get_actual_qos() const
+{
+  auto qos = std::make_shared<rmw_qos_profile_t>();
+
+  rcl_ret_t status = rcl_publisher_get_actual_qos(
+    &publisher_handle_,
+    qos.get());
+  if (RCL_RET_PUBLISHER_INVALID == status) {
+    rcl_reset_error();  /* next call will reset error message if not context */
+    if (rcl_publisher_is_valid_except_context(&publisher_handle_)) {
+      rcl_context_t * context = rcl_publisher_get_context(&publisher_handle_);
+      if (nullptr != context && !rcl_context_is_valid(context)) {
+        /* publisher is invalid due to context being shutdown */
+        return 0;
+      }
+    }
+  }
+  if (RCL_RET_OK != status) {
+    rclcpp::exceptions::throw_from_rcl_error(status, "failed to get qos settings");
+  }
+  return qos;
+}
+
 bool
 PublisherBase::operator==(const rmw_gid_t & gid) const
 {