Add QoS profile query parameters (#133)

Author: sea-bass

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"
 
@@ -66,6 +67,7 @@ class ImageTransportImageStreamer : public ImageStreamer
   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

@@ -29,6 +29,7 @@ class RosCompressedStreamer : public ImageStreamer
   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

@@ -24,6 +24,7 @@ ImageTransportImageStreamer::ImageTransportImageStreamer(const async_web_server_
   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()
@@ -46,7 +47,22 @@ 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

@@ -8,6 +8,7 @@ RosCompressedStreamer::RosCompressedStreamer(const async_web_server_cpp::HttpReq
   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()
@@ -17,9 +18,23 @@ 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, std::bind(&RosCompressedStreamer::imageCallback, this, std::placeholders::_1));
+      compressed_topic, qos, std::bind(&RosCompressedStreamer::imageCallback, this, std::placeholders::_1));
 }
 
 void RosCompressedStreamer::restreamFrame(double max_age)

src/utils.cpp

@@ -0,0 +1,26 @@
+#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