image_proc: consistent image_transport (#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);