diff --git a/src/streamers/image_transport_streamer.cpp b/src/streamers/image_transport_streamer.cpp index 5369276..637250a 100644 --- a/src/streamers/image_transport_streamer.cpp +++ b/src/streamers/image_transport_streamer.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include @@ -54,8 +55,11 @@ #include "image_transport/image_transport.hpp" #include "image_transport/transport_hints.hpp" #include "rclcpp/node.hpp" +#include "rclcpp/logger.hpp" #include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" #include "rmw/qos_profiles.h" +#include "rmw/types.h" #include "sensor_msgs/msg/image.hpp" #include "web_video_server/streamer.hpp" @@ -84,6 +88,78 @@ std::vector get_image_topics(rclcpp::Node & node) return result; } +std::optional detect_publisher_qos( + const rclcpp::Node::SharedPtr & node, + const rclcpp::Logger & logger, + const std::string & topic) +{ + RCLCPP_INFO(logger, "Attempting to auto-detect QoS for topic: %s", topic.c_str()); + + const auto topic_endpoint_info_array = node->get_publishers_info_by_topic(topic); + if (topic_endpoint_info_array.empty()) { + RCLCPP_WARN(logger, "No publishers found for topic: %s", topic.c_str()); + return std::nullopt; + } + + // Use the first publisher's QoS as reference. + const auto & endpoint_info = topic_endpoint_info_array.front(); + const auto qos_profile = endpoint_info.qos_profile(); + + const std::string reliability = + (qos_profile.reliability() == rclcpp::ReliabilityPolicy::Reliable) ? "RELIABLE" : "BEST_EFFORT"; + const std::string durability = + (qos_profile.durability() == rclcpp::DurabilityPolicy::TransientLocal) ? + "TRANSIENT_LOCAL" : "VOLATILE"; + + RCLCPP_INFO( + logger, "Detected QoS - Reliability: %s, Durability: %s, History depth: %zu", + reliability.c_str(), durability.c_str(), qos_profile.depth()); + + // Convert rclcpp QoS to rmw QoS profile. + auto rmw_qos = rmw_qos_profile_default; + rmw_qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + rmw_qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + rmw_qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + rmw_qos.depth = qos_profile.depth(); + + if (qos_profile.reliability() == rclcpp::ReliabilityPolicy::Reliable) { + rmw_qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + } + if (qos_profile.durability() == rclcpp::DurabilityPolicy::TransientLocal) { + rmw_qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + } + + return rmw_qos; +} + +std::optional get_qos_profile( + const rclcpp::Node::SharedPtr & node, + const rclcpp::Logger & logger, + const std::string & profile_name, + const std::string & topic) +{ + if (profile_name == "auto") { + auto qos_profile = detect_publisher_qos(node, logger, topic); + if (!qos_profile) { + RCLCPP_WARN( + logger, "Could not auto-detect QoS for topic %s. Using default profile.", topic.c_str()); + return rmw_qos_profile_default; + } + RCLCPP_INFO(logger, "Using auto-detected QoS profile for topic %s", topic.c_str()); + return qos_profile; + } + + RCLCPP_INFO( + logger, "Using specified QoS profile %s for topic %s", profile_name.c_str(), topic.c_str()); + auto qos_profile = web_video_server::get_qos_profile_from_name(profile_name); + if (!qos_profile) { + RCLCPP_ERROR( + logger, "Invalid QoS profile %s specified. Using default profile.", profile_name.c_str()); + return rmw_qos_profile_default; + } + return qos_profile; +} + } // namespace ImageTransportStreamerBase::ImageTransportStreamerBase( @@ -97,7 +173,7 @@ ImageTransportStreamerBase::ImageTransportStreamerBase( output_height_ = request.get_query_param_value_or_default("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"); + qos_profile_name_ = request.get_query_param_value_or_default("qos_profile", "auto"); } ImageTransportStreamerBase::~ImageTransportStreamerBase() @@ -133,18 +209,8 @@ void ImageTransportStreamerBase::start() } } - // Get QoS profile from query parameter - RCLCPP_INFO( - 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( - logger_, - "Invalid QoS profile %s specified. Using default profile.", - qos_profile_name_.c_str()); - } + // Get QoS profile based on user selection or auto-detect. + const auto qos_profile = get_qos_profile(node, logger_, qos_profile_name_, topic_); // Create subscriber image_sub_ = image_transport::create_subscription(