Add qos option to override qos

image_transport/CMakeLists.txt

@@ -122,6 +122,11 @@ if(BUILD_TESTING)
     target_link_libraries(${PROJECT_NAME}-remapping ${PROJECT_NAME})
   endif()
 
+  ament_add_gtest(${PROJECT_NAME}-qos_override test/test_qos_override.cpp)
+  if(TARGET ${PROJECT_NAME}-qos_override)
+    target_link_libraries(${PROJECT_NAME}-qos_override ${PROJECT_NAME})
+  endif()
+
   ament_add_gtest(${PROJECT_NAME}-single_subscriber_publisher test/test_single_subscriber_publisher.cpp)
   if(TARGET ${PROJECT_NAME}-single_subscriber_publisher)
     target_link_libraries(${PROJECT_NAME}-single_subscriber_publisher ${PROJECT_NAME})

image_transport/include/image_transport/image_transport.hpp

@@ -53,7 +53,8 @@ IMAGE_TRANSPORT_PUBLIC
 Publisher create_publisher(
   rclcpp::Node * node,
   const std::string & base_topic,
-  rmw_qos_profile_t custom_qos = rmw_qos_profile_default);
+  rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
+  rclcpp::PublisherOptions options = rclcpp::PublisherOptions());
 
 /**
  * \brief Subscribe to an image topic, free function version.

image_transport/include/image_transport/publisher.hpp

@@ -73,7 +73,8 @@ class Publisher
     rclcpp::Node * nh,
     const std::string & base_topic,
     PubLoaderPtr loader,
-    rmw_qos_profile_t custom_qos);
+    rmw_qos_profile_t custom_qos,
+    rclcpp::PublisherOptions options = rclcpp::PublisherOptions());
 
   /*!
    * \brief Returns the number of subscribers that are currently connected to

image_transport/include/image_transport/publisher_plugin.hpp

@@ -65,9 +65,10 @@ class PublisherPlugin
   void advertise(
     rclcpp::Node * nh,
     const std::string & base_topic,
-    rmw_qos_profile_t custom_qos = rmw_qos_profile_default)
+    rmw_qos_profile_t custom_qos = rmw_qos_profile_default,
+    rclcpp::PublisherOptions options = rclcpp::PublisherOptions())
   {
-    advertiseImpl(nh, base_topic, custom_qos);
+    advertiseImpl(nh, base_topic, custom_qos, options);
   }
 
   /**
@@ -136,6 +137,19 @@ class PublisherPlugin
   virtual void advertiseImpl(
     rclcpp::Node * nh, const std::string & base_topic,
     rmw_qos_profile_t custom_qos) = 0;
+
+  virtual void advertiseImpl(
+    rclcpp::Node * node,
+    const std::string & base_topic,
+    rmw_qos_profile_t custom_qos,
+    rclcpp::PublisherOptions options)
+  {
+    (void) options;
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "PublisherPlugin::advertiseImpl with four arguments has not been overridden");
+    this->advertiseImpl(node, base_topic, custom_qos);
+  }
 };
 
 }  // namespace image_transport

image_transport/include/image_transport/raw_publisher.hpp

@@ -66,6 +66,15 @@ class RawPublisher : public SimplePublisherPlugin<sensor_msgs::msg::Image>
   {
     return base_topic;
   }
+
+  void advertiseImpl(
+    rclcpp::Node * node,
+    const std::string & base_topic,
+    rmw_qos_profile_t custom_qos,
+    rclcpp::PublisherOptions options) override
+  {
+    this->advertiseImplWithOptions(node, base_topic, custom_qos, options);
+  }
 };
 
 }  // namespace image_transport

image_transport/include/image_transport/simple_publisher_plugin.hpp

@@ -98,9 +98,9 @@ class SimplePublisherPlugin : public PublisherPlugin
   }
 
 protected:
-  virtual void advertiseImpl(
+  void advertiseImpl(
     rclcpp::Node * node, const std::string & base_topic,
-    rmw_qos_profile_t custom_qos)
+    rmw_qos_profile_t custom_qos) override
   {
     std::string transport_topic = getTopicToAdvertise(base_topic);
     simple_impl_ = std::make_unique<SimplePublisherPluginImpl>(node);
@@ -110,6 +110,20 @@ class SimplePublisherPlugin : public PublisherPlugin
     simple_impl_->pub_ = node->create_publisher<M>(transport_topic, qos);
   }
 
+  void advertiseImplWithOptions(
+    rclcpp::Node * node,
+    const std::string & base_topic,
+    rmw_qos_profile_t custom_qos,
+    rclcpp::PublisherOptions options)
+  {
+    std::string transport_topic = getTopicToAdvertise(base_topic);
+    simple_impl_ = std::make_unique<SimplePublisherPluginImpl>(node);
+
+    RCLCPP_DEBUG(simple_impl_->logger_, "getTopicToAdvertise: %s", transport_topic.c_str());
+    auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos);
+    simple_impl_->pub_ = node->create_publisher<M>(transport_topic, qos, options);
+  }
+
   //! Generic function for publishing the internal message type.
   typedef std::function<void (const M &)> PublishFn;
 

image_transport/src/image_transport.cpp

@@ -59,9 +59,10 @@ static Impl * kImpl = new Impl();
 Publisher create_publisher(
   rclcpp::Node * node,
   const std::string & base_topic,
-  rmw_qos_profile_t custom_qos)
+  rmw_qos_profile_t custom_qos,
+  rclcpp::PublisherOptions options)
 {
-  return Publisher(node, base_topic, kImpl->pub_loader_, custom_qos);
+  return Publisher(node, base_topic, kImpl->pub_loader_, custom_qos, options);
 }
 
 Subscriber create_subscription(

image_transport/src/publisher.cpp

@@ -98,7 +98,8 @@ struct Publisher::Impl
 
 Publisher::Publisher(
   rclcpp::Node * node, const std::string & base_topic,
-  PubLoaderPtr loader, rmw_qos_profile_t custom_qos)
+  PubLoaderPtr loader, rmw_qos_profile_t custom_qos,
+  rclcpp::PublisherOptions options)
 : impl_(std::make_shared<Impl>(node))
 {
   // Resolve the name explicitly because otherwise the compressed topics don't remap
@@ -124,7 +125,7 @@ Publisher::Publisher(
 
     try {
       auto pub = loader->createUniqueInstance(lookup_name);
-      pub->advertise(node, image_topic, custom_qos);
+      pub->advertise(node, image_topic, custom_qos, options);
       impl_->publishers_.push_back(std::move(pub));
     } catch (const std::runtime_error & e) {
       RCLCPP_ERROR(

image_transport/test/test_publisher.cpp

@@ -46,7 +46,7 @@ class TestPublisher : public ::testing::Test
   rclcpp::Node::SharedPtr node_;
 };
 
-TEST_F(TestPublisher, Publisher) {
+TEST_F(TestPublisher, publisher) {
   auto pub = image_transport::create_publisher(node_.get(), "camera/image");
   EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/image"), 1u);
   pub.shutdown();
@@ -56,12 +56,12 @@ TEST_F(TestPublisher, Publisher) {
   pub.publish(sensor_msgs::msg::Image::ConstSharedPtr());
 }
 
-TEST_F(TestPublisher, ImageTransportPublisher) {
+TEST_F(TestPublisher, image_transport_publisher) {
   image_transport::ImageTransport it(node_);
   auto pub = it.advertise("camera/image", 1);
 }
 
-TEST_F(TestPublisher, CameraPublisher) {
+TEST_F(TestPublisher, camera_publisher) {
   auto camera_pub = image_transport::create_camera_publisher(node_.get(), "camera/image");
   EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/image"), 1u);
   EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/camera_info"), 1u);
@@ -75,7 +75,7 @@ TEST_F(TestPublisher, CameraPublisher) {
     sensor_msgs::msg::CameraInfo::ConstSharedPtr());
 }
 
-TEST_F(TestPublisher, ImageTransportCameraPublisher) {
+TEST_F(TestPublisher, image_transport_camera_publisher) {
   image_transport::ImageTransport it(node_);
   auto pub = it.advertiseCamera("camera/image", 1);
 }

image_transport/test/test_qos_override.cpp

@@ -0,0 +1,162 @@
+// Copyright (c) 2022 Open Source Robotics Foundation, Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the Willow Garage nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "image_transport/image_transport.hpp"
+
+class TestQosOverride : public ::testing::Test
+{
+protected:
+  void SetUp()
+  {
+    pub_node_ = rclcpp::Node::make_shared("test_publisher");
+    qos_override_pub_node_ = rclcpp::Node::make_shared(
+      "test_qos_override_publisher", rclcpp::NodeOptions().parameter_overrides(
+    {
+      rclcpp::Parameter(
+        "qos_overrides./camera/image.publisher.reliability", "best_effort"),
+    }));
+    sub_node_ = rclcpp::Node::make_shared("test_subscriber");
+    qos_override_sub_node_ = rclcpp::Node::make_shared(
+      "test_qos_override_subscriber", rclcpp::NodeOptions().parameter_overrides(
+    {
+      rclcpp::Parameter(
+        "qos_overrides./camera/image.subscription.reliability", "best_effort"),
+    }));
+  }
+
+  rclcpp::Node::SharedPtr pub_node_;
+  rclcpp::Node::SharedPtr qos_override_pub_node_;
+  rclcpp::Node::SharedPtr sub_node_;
+  rclcpp::Node::SharedPtr qos_override_sub_node_;
+};
+
+TEST_F(TestQosOverride, qos_override_publisher_without_options) {
+  auto pub =
+    image_transport::create_publisher(pub_node_.get(), "camera/image", rmw_qos_profile_default);
+  auto endpoint_info_vec = pub_node_->get_publishers_info_by_topic("camera/image");
+  EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable);
+  pub.shutdown();
+
+  pub = image_transport::create_publisher(
+    qos_override_pub_node_.get(), "camera/image", rmw_qos_profile_default);
+
+  endpoint_info_vec = qos_override_pub_node_->get_publishers_info_by_topic("camera/image");
+  EXPECT_EQ(
+    endpoint_info_vec[0].qos_profile().reliability(),
+    rclcpp::ReliabilityPolicy::Reliable);
+  pub.shutdown();
+}
+
+TEST_F(TestQosOverride, qos_override_publisher_with_options) {
+  rclcpp::PublisherOptions options;
+  options.qos_overriding_options = rclcpp::QosOverridingOptions(
+  {
+    rclcpp::QosPolicyKind::Depth,
+    rclcpp::QosPolicyKind::Durability,
+    rclcpp::QosPolicyKind::History,
+    rclcpp::QosPolicyKind::Reliability,
+  });
+
+  auto pub = image_transport::create_publisher(
+    pub_node_.get(), "camera/image", rmw_qos_profile_default, options);
+  auto endpoint_info_vec = pub_node_->get_publishers_info_by_topic("camera/image");
+  EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable);
+  pub.shutdown();
+
+  pub = image_transport::create_publisher(
+    qos_override_pub_node_.get(), "camera/image", rmw_qos_profile_default, options);
+
+  endpoint_info_vec = qos_override_pub_node_->get_publishers_info_by_topic("camera/image");
+  EXPECT_EQ(
+    endpoint_info_vec[0].qos_profile().reliability(),
+    rclcpp::ReliabilityPolicy::BestEffort);
+  pub.shutdown();
+}
+
+TEST_F(TestQosOverride, qos_override_subscriber_without_options) {
+  std::function<void(const sensor_msgs::msg::Image::ConstSharedPtr & msg)> fcn =
+    [](const auto & msg) {(void)msg;};
+
+  auto sub = image_transport::create_subscription(
+    sub_node_.get(), "camera/image", fcn, "raw", rmw_qos_profile_default);
+  auto endpoint_info_vec = sub_node_->get_subscriptions_info_by_topic("camera/image");
+  EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable);
+  sub.shutdown();
+
+  sub = image_transport::create_subscription(
+    qos_override_sub_node_.get(), "camera/image", fcn, "raw", rmw_qos_profile_default);
+
+  endpoint_info_vec = qos_override_sub_node_->get_subscriptions_info_by_topic("camera/image");
+  EXPECT_EQ(
+    endpoint_info_vec[0].qos_profile().reliability(),
+    rclcpp::ReliabilityPolicy::Reliable);
+}
+
+TEST_F(TestQosOverride, qos_override_subscriber_with_options) {
+  std::function<void(const sensor_msgs::msg::Image::ConstSharedPtr & msg)> fcn =
+    [](const auto & msg) {(void)msg;};
+
+  rclcpp::SubscriptionOptions options;
+  options.qos_overriding_options = rclcpp::QosOverridingOptions(
+  {
+    rclcpp::QosPolicyKind::Depth,
+    rclcpp::QosPolicyKind::Durability,
+    rclcpp::QosPolicyKind::History,
+    rclcpp::QosPolicyKind::Reliability,
+  });
+
+  auto sub = image_transport::create_subscription(
+    sub_node_.get(), "camera/image", fcn, "raw", rmw_qos_profile_default, options);
+  auto endpoint_info_vec = sub_node_->get_subscriptions_info_by_topic("camera/image");
+  EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable);
+  sub.shutdown();
+
+  sub = image_transport::create_subscription(
+    qos_override_sub_node_.get(), "camera/image", fcn, "raw", rmw_qos_profile_default, options);
+
+  endpoint_info_vec = qos_override_sub_node_->get_subscriptions_info_by_topic("camera/image");
+  EXPECT_EQ(
+    endpoint_info_vec[0].qos_profile().reliability(),
+    rclcpp::ReliabilityPolicy::BestEffort);
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  testing::InitGoogleTest(&argc, argv);
+  int ret = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return ret;
+}

image_transport/test/test_subscriber.cpp

@@ -35,7 +35,7 @@
 
 #include "image_transport/image_transport.hpp"
 
-class TestPublisher : public ::testing::Test
+class TestSubscriber : public ::testing::Test
 {
 protected:
   void SetUp()
@@ -46,7 +46,7 @@ class TestPublisher : public ::testing::Test
   rclcpp::Node::SharedPtr node_;
 };
 
-TEST_F(TestPublisher, construction_and_destruction) {
+TEST_F(TestSubscriber, construction_and_destruction) {
   std::function<void(const sensor_msgs::msg::Image::ConstSharedPtr & msg)> fcn =
     [](const auto & msg) {(void)msg;};
 
@@ -56,7 +56,7 @@ TEST_F(TestPublisher, construction_and_destruction) {
   executor.spin_node_some(node_);
 }
 
-TEST_F(TestPublisher, shutdown) {
+TEST_F(TestSubscriber, shutdown) {
   std::function<void(const sensor_msgs::msg::Image::ConstSharedPtr & msg)> fcn =
     [](const auto & msg) {(void)msg;};
 
@@ -66,7 +66,7 @@ TEST_F(TestPublisher, shutdown) {
   EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/image"), 0u);
 }
 
-TEST_F(TestPublisher, camera_sub_shutdown) {
+TEST_F(TestSubscriber, camera_sub_shutdown) {
   std::function<void(
       const sensor_msgs::msg::Image::ConstSharedPtr &,
       const sensor_msgs::msg::CameraInfo::ConstSharedPtr &)> fcn =