From bc9fe6380860f491a31544ea1a46c31364f430a1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?=
 <alejandro@openrobotics.org>
Date: Wed, 6 Mar 2024 13:36:16 +0100
Subject: [PATCH] Select QoS reliability policy in DepthCloud Plugin (#1159)
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>
(cherry picked from commit a76cf91b1b5a4d21c5b6e2405fae99799318f363)
---
 .../depth_cloud/depth_cloud_display.hpp       |  4 ++
 .../depth_cloud/depth_cloud_display.cpp       | 38 ++++++++++++++++++-
 2 files changed, 40 insertions(+), 2 deletions(-)

diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp
index 6ddd8e1cd..a344a3804 100644
--- a/rviz_default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp
+++ b/rviz_default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp
@@ -106,6 +106,8 @@ protected Q_SLOTS:
 
   void transformerChangedCallback();
 
+  void updateQosProfile();
+
   // Property callbacks
   virtual void updateTopic();
   virtual void updateTopicFilter();
@@ -160,6 +162,8 @@ protected Q_SLOTS:
   rclcpp::Time subscription_start_time_;
 
   // RVIZ properties
+  rviz_common::properties::EditableEnumProperty * reliability_policy_property_;
+  rmw_qos_profile_t qos_profile_;
   rviz_common::properties::Property * topic_filter_property_;
   rviz_common::properties::IntProperty * queue_size_property_;
   rviz_common::properties::BoolProperty * use_auto_size_property_;
diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.cpp
index 72aa8e318..5a3c159dc 100644
--- a/rviz_default_plugins/src/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.cpp
+++ b/rviz_default_plugins/src/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.cpp
@@ -84,6 +84,18 @@ DepthCloudDisplay::DepthCloudDisplay()
   QRegExp depth_filter("depth");
   depth_filter.setCaseSensitivity(Qt::CaseInsensitive);
 
+  reliability_policy_property_ = new rviz_common::properties::EditableEnumProperty(
+    "Reliability Policy",
+    "Best effort",
+    "Set the reliability policy: When choosing 'Best effort', no guarantee will be given that the "
+    "messages will be delivered, choosing 'Reliable', messages will be sent until received.", this,
+    SLOT(updateQosProfile()));
+  reliability_policy_property_->addOption("System Default");
+  reliability_policy_property_->addOption("Reliable");
+  reliability_policy_property_->addOption("Best effort");
+
+  qos_profile_ = rmw_qos_profile_sensor_data;
+
   topic_filter_property_ =
     new rviz_common::properties::Property(
     "Topic Filter", true,
@@ -162,6 +174,26 @@ DepthCloudDisplay::DepthCloudDisplay()
     use_occlusion_compensation_property_, SLOT(updateOcclusionTimeOut()), this);
 }
 
+void DepthCloudDisplay::updateQosProfile()
+{
+  updateQueueSize();
+  qos_profile_ = rmw_qos_profile_default;
+  qos_profile_.depth = queue_size_;
+
+  auto policy = reliability_policy_property_->getString().toStdString();
+
+  if (policy == "Best effort") {
+    qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
+
+  } else if (policy == "Reliable") {
+    qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  } else {
+    qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
+  }
+
+  updateTopic();
+}
+
 void DepthCloudDisplay::transformerChangedCallback()
 {
   updateTopic();
@@ -225,6 +257,7 @@ void DepthCloudDisplay::updateQueueSize()
     depthmap_tf_filter_->setQueueSize(static_cast<uint32_t>(queue_size_property_->getInt()));
   }
   queue_size_ = queue_size_property_->getInt();
+  qos_profile_.depth = queue_size_;
 }
 
 void DepthCloudDisplay::updateUseAutoSize()
@@ -311,7 +344,8 @@ void DepthCloudDisplay::subscribe()
       depthmap_sub_->subscribe(
         rviz_ros_node_->get_raw_node().get(),
         depthmap_topic,
-        depthmap_transport);
+        depthmap_transport,
+        qos_profile_);
 
       depthmap_tf_filter_ =
         std::make_shared<tf2_ros::MessageFilter<sensor_msgs::msg::Image,
@@ -353,7 +387,7 @@ void DepthCloudDisplay::subscribe()
         // subscribe to color image topic
         rgb_sub_->subscribe(
           rviz_ros_node_->get_raw_node().get(),
-          color_topic, color_transport, rclcpp::SensorDataQoS().get_rmw_qos_profile());
+          color_topic, color_transport, qos_profile_);
 
         // connect message filters to synchronizer
         sync_depth_color_->connectInput(*depthmap_tf_filter_, *rgb_sub_);