Add QoS profile query parameters (RobotWebTools#133)

CMakeLists.txt

@@ -51,6 +51,7 @@ add_executable(${PROJECT_NAME}
   src/ros_compressed_streamer.cpp
   src/jpeg_streamers.cpp
   src/png_streamers.cpp
+  src/utils.cpp
 )
 
 ament_target_dependencies(${PROJECT_NAME}

include/web_video_server/image_streamer.h

@@ -5,6 +5,7 @@
 #include <image_transport/image_transport.hpp>
 #include <image_transport/transport_hints.hpp>
 #include <opencv2/opencv.hpp>
+#include "web_video_server/utils.h"
 #include "async_web_server_cpp/http_server.hpp"
 #include "async_web_server_cpp/http_request.hpp"
 
@@ -65,6 +66,7 @@ namespace web_video_server
     int output_height_;
     bool invert_;
     std::string default_transport_;
+    std::string qos_profile_name_;
 
     rclcpp::Time last_frame;
     cv::Mat output_size_image;

include/web_video_server/ros_compressed_streamer.h

@@ -32,6 +32,7 @@ namespace web_video_server
     rclcpp::Time last_frame;
     sensor_msgs::msg::CompressedImage::ConstSharedPtr last_msg;
     boost::mutex send_mutex_;
+    std::string qos_profile_name_;
   };
 
   class RosCompressedStreamerType: public ImageStreamerType

include/web_video_server/utils.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include <string>
+#include <optional>
+#include "rmw/qos_profiles.h"
+
+namespace web_video_server
+{
+
+/**
+ * @brief Gets a QoS profile given an input name, if valid.
+ * @param name The name of the QoS profile name.
+ * @return An optional containing the matching QoS profile.
+ */
+  std::optional < rmw_qos_profile_t > get_qos_profile_from_name(const std::string name);
+
+}  // namespace web_video_server

src/image_streamer.cpp

@@ -26,6 +26,7 @@ ImageTransportImageStreamer::ImageTransportImageStreamer(
   output_height_ = request.get_query_param_value_or_default<int>("height", -1);
   invert_ = request.has_query_param("invert");
   default_transport_ = request.get_query_param_value_or_default("default_transport", "raw");
+  qos_profile_name_ = request.get_query_param_value_or_default("qos_profile", "default");
 }
 
 ImageTransportImageStreamer::~ImageTransportImageStreamer()
@@ -48,7 +49,24 @@ void ImageTransportImageStreamer::start()
       break;
     }
   }
-  image_sub_ = it_.subscribe(topic_, 1, &ImageTransportImageStreamer::imageCallback, this, &hints);
+
+  // Get QoS profile from query parameter
+  RCLCPP_INFO(nh_->get_logger(), "Streaming topic %s with QoS profile %s", topic_.c_str(),
+      qos_profile_name_.c_str());
+  auto qos_profile = get_qos_profile_from_name(qos_profile_name_);
+  if (!qos_profile) {
+    qos_profile = rmw_qos_profile_default;
+    RCLCPP_ERROR(
+      nh_->get_logger(),
+      "Invalid QoS profile %s specified. Using default profile.",
+      qos_profile_name_.c_str());
+  }
+
+  // Create subscriber
+  image_sub_ = image_transport::create_subscription(
+      nh_.get(), topic_,
+      std::bind(&ImageTransportImageStreamer::imageCallback, this, std::placeholders::_1),
+      default_transport_, qos_profile.value());
 }
 
 void ImageTransportImageStreamer::initialize(const cv::Mat &)

src/ros_compressed_streamer.cpp

@@ -9,6 +9,7 @@ RosCompressedStreamer::RosCompressedStreamer(
 :ImageStreamer(request, connection, nh), stream_(std::bind(&rclcpp::Node::now, nh), connection)
 {
   stream_.sendInitialHeader();
+  qos_profile_name_ = request.get_query_param_value_or_default("qos_profile", "default");
 }
 
 RosCompressedStreamer::~RosCompressedStreamer()
@@ -19,9 +20,25 @@ RosCompressedStreamer::~RosCompressedStreamer()
 
 void RosCompressedStreamer::start()
 {
-  std::string compressed_topic = topic_ + "/compressed";
+  const std::string compressed_topic = topic_ + "/compressed";
+
+  // Get QoS profile from query parameter
+  RCLCPP_INFO(nh_->get_logger(), "Streaming topic %s with QoS profile %s", compressed_topic.c_str(),
+      qos_profile_name_.c_str());
+  auto qos_profile = get_qos_profile_from_name(qos_profile_name_);
+  if (!qos_profile) {
+    qos_profile = rmw_qos_profile_default;
+    RCLCPP_ERROR(
+      nh_->get_logger(),
+      "Invalid QoS profile %s specified. Using default profile.",
+      qos_profile_name_.c_str());
+  }
+
+  // Create subscriber
+  const auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.value().history, 1),
+      qos_profile.value());
   image_sub_ = nh_->create_subscription<sensor_msgs::msg::CompressedImage>(
-    compressed_topic, 1,
+      compressed_topic, qos,
       std::bind(&RosCompressedStreamer::imageCallback, this, std::placeholders::_1));
 }
 

src/utils.cpp

@@ -0,0 +1,19 @@
+#include "web_video_server/utils.h"
+
+namespace web_video_server
+{
+
+std::optional<rmw_qos_profile_t> get_qos_profile_from_name(const std::string name)
+{
+  if (name == "default") {
+    return rmw_qos_profile_default;
+  } else if (name == "system_default") {
+    return rmw_qos_profile_system_default;
+  } else if (name == "sensor_data") {
+    return rmw_qos_profile_sensor_data;
+  } else {
+    return std::nullopt;
+  }
+}
+
+}  // namespace web_video_server