resize/recify: consistent image_transport (ros-perception#883)

* support image_transport parameter
 * proper renaming so compressed/etc topics work as expected

Additional minor fixes:
 * rename interpolation -> interpolation_ for consistency
* move parameter declaration BEFORE we create a publisher (and possibly
get a subscriber created in connect callback)
* put the getTopicQosProfile() for publisher right in front of publisher
declaration for clarity

image_proc/include/image_proc/rectify.hpp

@@ -34,6 +34,7 @@
 #define IMAGE_PROC__RECTIFY_HPP_
 
 #include <mutex>
+#include <string>
 
 #include "image_geometry/pinhole_camera_model.hpp"
 
@@ -55,7 +56,8 @@ class RectifyNode
   image_transport::CameraSubscriber sub_camera_;
 
   int queue_size_;
-  int interpolation;
+  int interpolation_;
+  std::string image_topic_;
   image_transport::Publisher pub_rect_;
 
   // Processing state (note: only safe because we're using single-threaded NodeHandle!)

image_proc/include/image_proc/resize.hpp

@@ -34,6 +34,7 @@
 #define IMAGE_PROC__RESIZE_HPP_
 
 #include <mutex>
+#include <string>
 
 #include <image_transport/image_transport.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -59,6 +60,7 @@ class ResizeNode
   double scale_width_;
   int height_;
   int width_;
+  std::string image_topic_;
 
   cv_bridge::CvImage scaled_cv_;
 

image_proc/src/rectify.cpp

@@ -32,6 +32,7 @@
 
 #include <functional>
 #include <mutex>
+#include <string>
 
 #include "cv_bridge/cv_bridge.hpp"
 #include "tracetools_image_pipeline/tracetools.h"
@@ -51,9 +52,16 @@ namespace image_proc
 RectifyNode::RectifyNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("RectifyNode", options)
 {
-  auto qos_profile = getTopicQosProfile(this, "image");
+  // 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", false);
+
   queue_size_ = this->declare_parameter("queue_size", 5);
-  interpolation = this->declare_parameter("interpolation", 1);
+  interpolation_ = this->declare_parameter("interpolation", 1);
 
   // Create publisher with connect callback
   rclcpp::PublisherOptions pub_options;
@@ -63,13 +71,17 @@ RectifyNode::RectifyNode(const rclcpp::NodeOptions & options)
       if (pub_rect_.getNumSubscribers() == 0) {
         sub_camera_.shutdown();
       } else if (!sub_camera_) {
-        auto qos_profile = getTopicQosProfile(this, "image");
+        auto qos_profile = getTopicQosProfile(this, image_topic_);
+        image_transport::TransportHints hints(this);
         sub_camera_ = image_transport::create_camera_subscription(
-          this, "image", std::bind(
+          this, image_topic_, std::bind(
             &RectifyNode::imageCb,
-            this, std::placeholders::_1, std::placeholders::_2), "raw", qos_profile);
+            this, 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_rect_ = image_transport::create_publisher(this, "image_rect", qos_profile, pub_options);
 }
 
@@ -135,7 +147,7 @@ void RectifyNode::imageCb(
   cv::Mat rect;
 
   // Rectify and publish
-  model_.rectifyImage(image, rect, interpolation);
+  model_.rectifyImage(image, rect, interpolation_);
 
   // Allocate new rectified image message
   sensor_msgs::msg::Image::SharedPtr rect_msg =

image_proc/src/resize.cpp

@@ -32,6 +32,7 @@
 
 #include <functional>
 #include <memory>
+#include <string>
 
 #include "cv_bridge/cv_bridge.hpp"
 #include "tracetools_image_pipeline/tracetools.h"
@@ -51,7 +52,22 @@ namespace image_proc
 ResizeNode::ResizeNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("ResizeNode", options)
 {
-  auto qos_profile = getTopicQosProfile(this, "image/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/image_raw", false);
+
+  // Declare parameters before we setup any publishers or subscribers
+  interpolation_ = this->declare_parameter("interpolation", 1);
+  use_scale_ = this->declare_parameter("use_scale", true);
+  scale_height_ = this->declare_parameter("scale_height", 1.0);
+  scale_width_ = this->declare_parameter("scale_width", 1.0);
+  height_ = this->declare_parameter("height", -1);
+  width_ = this->declare_parameter("width", -1);
+
   // Create image pub with connection callback
   rclcpp::PublisherOptions pub_options;
   pub_options.event_callbacks.matched_callback =
@@ -60,24 +76,20 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options)
       if (pub_image_.getNumSubscribers() == 0) {
         sub_image_.shutdown();
       } else if (!sub_image_) {
-        auto qos_profile = getTopicQosProfile(this, "image/image_raw");
+        // Match the subscriber QoS
+        auto qos_profile = getTopicQosProfile(this, image_topic_);
+        image_transport::TransportHints hints(this);
         sub_image_ = image_transport::create_camera_subscription(
-          this, "image/image_raw",
+          this, image_topic_,
           std::bind(
             &ResizeNode::imageCb, this,
             std::placeholders::_1,
-            std::placeholders::_2), "raw", qos_profile);
+            std::placeholders::_2), hints.getTransport(), qos_profile);
       }
     };
+  auto qos_profile = getTopicQosProfile(this, image_topic_);
   pub_image_ = image_transport::create_camera_publisher(
     this, "resize/image_raw", qos_profile, pub_options);
-
-  interpolation_ = this->declare_parameter("interpolation", 1);
-  use_scale_ = this->declare_parameter("use_scale", true);
-  scale_height_ = this->declare_parameter("scale_height", 1.0);
-  scale_width_ = this->declare_parameter("scale_width", 1.0);
-  height_ = this->declare_parameter("height", -1);
-  width_ = this->declare_parameter("width", -1);
 }
 
 void ResizeNode::imageCb(