Add QoS profile parameter to image bridge

ros_gz_image/README.md

@@ -9,3 +9,14 @@ For compressed images, install
 and the bridge will publish `/compressed` images. The same goes for other
 `image_transport` plugins.
 
+To run the bridge from the command line:
+
+```shell
+ros2 run ros_gz_image image_bridge /topic1 /topic2
+```
+
+You can also modify the [Quality of Service (QoS) policy](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-policies) used to publish images using an additional `qos` ROS parameter. For example:
+
+```shell
+ros2 run ros_gz_image image_bridge /topic1 /topic2 --ros-args qos:=sensor_data
+```

ros_gz_image/src/image_bridge.cpp

@@ -12,32 +12,50 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <gz/transport/Node.hh>
+#include <rmw/qos_profiles.h>
 
 #include <iostream>
 #include <memory>
 #include <string>
 #include <vector>
 
-#include <gz/transport/Node.hh>
 #include <rclcpp/rclcpp.hpp>
 #include <image_transport/image_transport.hpp>
 #include <ros_gz_bridge/convert.hpp>
 
 //////////////////////////////////////////////////
-/// \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 +95,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 +109,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