From ebd3bf13226629149050650112fa8a9fcf0ab069 Mon Sep 17 00:00:00 2001
From: Sebastian Castro <sebas.a.castro@gmail.com>
Date: Mon, 5 Dec 2022 11:09:29 -0700
Subject: [PATCH] Add QoS profile parameter to image bridge

---
 ros_gz_image/src/image_bridge.cpp | 29 +++++++++++++++++++++++------
 1 file changed, 23 insertions(+), 6 deletions(-)

diff --git a/ros_gz_image/src/image_bridge.cpp b/ros_gz_image/src/image_bridge.cpp
index f72160f28..109d1da28 100644
--- a/ros_gz_image/src/image_bridge.cpp
+++ b/ros_gz_image/src/image_bridge.cpp
@@ -22,22 +22,39 @@
 #include <rclcpp/rclcpp.hpp>
 #include <image_transport/image_transport.hpp>
 #include <ros_gz_bridge/convert.hpp>
+#include <rmw/qos_profiles.h>
 
 //////////////////////////////////////////////////
-/// \brief Bridges one topic
+/// \brief Bridges one image topic
 class Handler
 {
 public:
   /// \brief Constructor
   /// \param[in] _topic Image base topic
-  /// \param[in] _it_node Pointer to image transport node
+  /// \param[in] _node Pointer to ROS node
   /// \param[in] _gz_node Pointer to Gazebo node
   Handler(
     const std::string & _topic,
-    std::shared_ptr<image_transport::ImageTransport> _it_node,
+    std::shared_ptr<rclcpp::Node> _node,
     std::shared_ptr<gz::transport::Node> _gz_node)
   {
-    this->ros_pub = _it_node->advertise(_topic, 1);
+    // Get QoS profile from parameter
+    rmw_qos_profile_t qos_profile = rmw_qos_profile_default;
+    const auto qos_str =
+      _node->get_parameter("qos").get_parameter_value().get<std::string>();
+    if (qos_str == "system_default") {
+      qos_profile = rmw_qos_profile_system_default;
+    } else if (qos_str == "sensor_data") {
+      qos_profile = rmw_qos_profile_sensor_data;
+    } else if (qos_str != "default") {
+      RCLCPP_ERROR(_node->get_logger(),
+        "Invalid QoS profile %s specified. Using default profile.",
+        qos_str.c_str());
+    }
+
+    // Create publishers and subscribers
+    this->ros_pub = image_transport::create_publisher(
+      _node.get(), _topic, qos_profile);
 
     _gz_node->Subscribe(_topic, &Handler::OnImage, this);
   }
@@ -77,7 +94,7 @@ int main(int argc, char * argv[])
 
   // ROS node
   auto node_ = rclcpp::Node::make_shared("ros_gz_image");
-  auto it_node = std::make_shared<image_transport::ImageTransport>(node_);
+  node_->declare_parameter("qos", "default");
 
   // Gazebo node
   auto gz_node = std::make_shared<gz::transport::Node>();
@@ -91,7 +108,7 @@ int main(int argc, char * argv[])
 
   // Create publishers and subscribers
   for (auto topic : args) {
-    handlers.push_back(std::make_shared<Handler>(topic, it_node, gz_node));
+    handlers.push_back(std::make_shared<Handler>(topic, node_, gz_node));
   }
 
   // Spin ROS and Gz until shutdown