From 860896aa66126daaf43947e8c08792c8b96c22b6 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Fri, 19 Jan 2024 11:28:49 -0500 Subject: [PATCH] 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 --- .../include/image_proc/crop_decimate.hpp | 1 + .../include/image_proc/crop_non_zero.hpp | 3 ++ image_proc/include/image_proc/debayer.hpp | 2 ++ image_proc/src/crop_decimate.cpp | 18 +++++++--- image_proc/src/crop_non_zero.cpp | 35 +++++++++++++++---- image_proc/src/debayer.cpp | 20 ++++++++--- 6 files changed, 64 insertions(+), 15 deletions(-) diff --git a/image_proc/include/image_proc/crop_decimate.hpp b/image_proc/include/image_proc/crop_decimate.hpp index 95e24e874..de9a7d65d 100644 --- a/image_proc/include/image_proc/crop_decimate.hpp +++ b/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( diff --git a/image_proc/include/image_proc/crop_non_zero.hpp b/image_proc/include/image_proc/crop_non_zero.hpp index 9b6b81bf2..8c2ee82d4 100644 --- a/image_proc/include/image_proc/crop_non_zero.hpp +++ b/image_proc/include/image_proc/crop_non_zero.hpp @@ -34,6 +34,7 @@ #define IMAGE_PROC__CROP_NON_ZERO_HPP_ #include +#include #include #include @@ -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_; diff --git a/image_proc/include/image_proc/debayer.hpp b/image_proc/include/image_proc/debayer.hpp index f4847fc2c..7752bb9b3 100644 --- a/image_proc/include/image_proc/debayer.hpp +++ b/image_proc/include/image_proc/debayer.hpp @@ -33,6 +33,7 @@ #ifndef IMAGE_PROC__DEBAYER_HPP_ #define IMAGE_PROC__DEBAYER_HPP_ +#include #include #include #include @@ -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; diff --git a/image_proc/src/crop_decimate.cpp b/image_proc/src/crop_decimate.cpp index 0cf0f4b44..084b98d4f 100644 --- a/image_proc/src/crop_decimate.cpp +++ b/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("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); } diff --git a/image_proc/src/crop_non_zero.cpp b/image_proc/src/crop_non_zero.cpp index f94f9f7ae..3add8add0 100644 --- a/image_proc/src/crop_non_zero.cpp +++ b/image_proc/src/crop_non_zero.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #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("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) diff --git a/image_proc/src/debayer.cpp b/image_proc/src/debayer.cpp index b784198f3..1c66d1da0 100644 --- a/image_proc/src/debayer.cpp +++ b/image_proc/src/debayer.cpp @@ -32,6 +32,7 @@ #include #include +#include #include @@ -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("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);