image_proc: consistent image_transport (ros-perception#884)

* consistent image_transport parameter for crop_decimate, crop_non_zero
and debayer nodes
* consistent remapping support for compressed/etc topics in all three
nodes
* add lazy subscription support to crop_non_zero

Additional minor fixes:

* put the getTopicQosProfile() for publisher right in front of publisher
declaration for clarity

image_proc/include/image_proc/crop_decimate.hpp

@@ -69,6 +69,7 @@ class CropDecimateNode : public rclcpp::Node
   int queue_size_;
   std::string target_frame_id_;
   int decimation_x_, decimation_y_, offset_x_, offset_y_, width_, height_;
+  std::string image_topic_;
   CropDecimateModes interpolation_;
 
   void imageCb(

image_proc/include/image_proc/crop_non_zero.hpp

@@ -34,6 +34,7 @@
 #define IMAGE_PROC__CROP_NON_ZERO_HPP_
 
 #include <mutex>
+#include <string>
 
 #include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -48,6 +49,8 @@ class CropNonZeroNode : public rclcpp::Node
   explicit CropNonZeroNode(const rclcpp::NodeOptions &);
 
 private:
+  std::string image_topic_;
+
   // Subscriptions
   image_transport::Subscriber sub_raw_;
 

image_proc/include/image_proc/debayer.hpp

@@ -33,6 +33,7 @@
 #ifndef IMAGE_PROC__DEBAYER_HPP_
 #define IMAGE_PROC__DEBAYER_HPP_
 
+#include <string>
 #include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/image.hpp>
@@ -50,6 +51,7 @@ class DebayerNode
   image_transport::Subscriber sub_raw_;
 
   int debayer_;
+  std::string image_topic_;
 
   int debayer_bilinear_ = 0;
   int debayer_edgeaware_ = 1;

image_proc/src/crop_decimate.cpp

@@ -108,7 +108,13 @@ void decimate(const cv::Mat & src, cv::Mat & dst, int decimation_x, int decimati
 CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options)
 : Node("CropNonZeroNode", options)
 {
-  auto qos_profile = getTopicQosProfile(this, "in/image_raw");
+  // TransportHints does not actually declare the parameter
+  this->declare_parameter<std::string>("image_transport", "raw");
+
+  // For compressed topics to remap appropriately, we need to pass a
+  // fully expanded and remapped topic name to image_transport
+  auto node_base = this->get_node_base_interface();
+  image_topic_ = node_base->resolve_topic_or_service_name("in/image_raw", false);
 
   queue_size_ = this->declare_parameter("queue_size", 5);
   target_frame_id_ = this->declare_parameter("target_frame_id", std::string());
@@ -135,13 +141,17 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options)
       if (pub_.getNumSubscribers() == 0) {
         sub_.shutdown();
       } else if (!sub_) {
-        auto qos_profile = getTopicQosProfile(this, "in/image_raw");
+        auto qos_profile = getTopicQosProfile(this, image_topic_);
+        image_transport::TransportHints hints(this);
         sub_ = image_transport::create_camera_subscription(
-          this, "in/image_raw", std::bind(
+          this, image_topic_, std::bind(
             &CropDecimateNode::imageCb, this,
-            std::placeholders::_1, std::placeholders::_2), "raw", qos_profile);
+            std::placeholders::_1, std::placeholders::_2), hints.getTransport(), qos_profile);
       }
     };
+
+  // Create publisher with same QoS as subscribed topic
+  auto qos_profile = getTopicQosProfile(this, image_topic_);
   pub_ = image_transport::create_camera_publisher(this, "out/image_raw", qos_profile, pub_options);
 }
 

image_proc/src/crop_non_zero.cpp

@@ -33,6 +33,7 @@
 #include <algorithm>
 #include <functional>
 #include <iterator>
+#include <string>
 #include <vector>
 
 #include "cv_bridge/cv_bridge.hpp"
@@ -52,14 +53,34 @@ namespace image_proc
 CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options)
 : Node("CropNonZeroNode", options)
 {
+  // TransportHints does not actually declare the parameter
+  this->declare_parameter<std::string>("image_transport", "raw");
+
+  // For compressed topics to remap appropriately, we need to pass a
+  // fully expanded and remapped topic name to image_transport
+  auto node_base = this->get_node_base_interface();
+  image_topic_ = node_base->resolve_topic_or_service_name("image_raw", false);
+
+  // Create image pub with connection callback
+  rclcpp::PublisherOptions pub_options;
+  pub_options.event_callbacks.matched_callback =
+    [this](rclcpp::MatchedInfo &)
+    {
+      if (pub_.getNumSubscribers() == 0) {
+        sub_raw_.shutdown();
+      } else if (!sub_raw_) {
+        auto qos_profile = getTopicQosProfile(this, image_topic_);
+        image_transport::TransportHints hints(this);
+        sub_raw_ = image_transport::create_subscription(
+          this, image_topic_, std::bind(
+            &CropNonZeroNode::imageCb, this,
+            std::placeholders::_1), hints.getTransport(), qos_profile);
+      }
+    };
+
+  // Create publisher with same QoS as subscribed topic
   auto qos_profile = getTopicQosProfile(this, "image_raw");
-  pub_ = image_transport::create_publisher(this, "image", qos_profile);
-  RCLCPP_INFO(this->get_logger(), "subscribe: %s", "image_raw");
-  sub_raw_ = image_transport::create_subscription(
-    this, "image_raw",
-    std::bind(
-      &CropNonZeroNode::imageCb,
-      this, std::placeholders::_1), "raw", qos_profile);
+  pub_ = image_transport::create_publisher(this, "image", qos_profile, pub_options);
 }
 
 void CropNonZeroNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg)

image_proc/src/debayer.cpp

@@ -32,6 +32,7 @@
 
 #include <functional>
 #include <memory>
+#include <string>
 
 #include <opencv2/imgproc/imgproc.hpp>
 
@@ -52,7 +53,14 @@ namespace image_proc
 DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)
 : Node("DebayerNode", options)
 {
-  auto qos_profile = getTopicQosProfile(this, "image_raw");
+  // TransportHints does not actually declare the parameter
+  this->declare_parameter<std::string>("image_transport", "raw");
+
+  // For compressed topics to remap appropriately, we need to pass a
+  // fully expanded and remapped topic name to image_transport
+  auto node_base = this->get_node_base_interface();
+  image_topic_ = node_base->resolve_topic_or_service_name("image_raw", false);
+
   // Create publisher options to setup callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
@@ -61,14 +69,18 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options)
       if (pub_mono_.getNumSubscribers() == 0 && pub_color_.getNumSubscribers() == 0) {
         sub_raw_.shutdown();
       } else if (!sub_raw_) {
-        auto qos_profile = getTopicQosProfile(this, "image_raw");
+        auto qos_profile = getTopicQosProfile(this, image_topic_);
+        image_transport::TransportHints hints(this);
         sub_raw_ = image_transport::create_subscription(
-          this, "image_raw",
+          this, image_topic_,
           std::bind(
             &DebayerNode::imageCb, this,
-            std::placeholders::_1), "raw", qos_profile);
+            std::placeholders::_1), hints.getTransport(), qos_profile);
       }
     };
+
+  // Create publisher with same QoS as subscribed topic
+  auto qos_profile = getTopicQosProfile(this, image_topic_);
   pub_mono_ = image_transport::create_publisher(this, "image_mono", qos_profile, pub_options);
   pub_color_ = image_transport::create_publisher(this, "image_color", qos_profile, pub_options);
   debayer_ = this->declare_parameter("debayer", 3);