From 705340999a8a570f0b6c1d8867d400eb75f3f562 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Fri, 19 Jan 2024 11:25:13 -0500 Subject: [PATCH] resize/recify: consistent image_transport (#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 | 4 ++- image_proc/include/image_proc/resize.hpp | 2 ++ image_proc/src/rectify.cpp | 24 ++++++++++++---- image_proc/src/resize.cpp | 34 +++++++++++++++-------- 4 files changed, 46 insertions(+), 18 deletions(-) diff --git a/image_proc/include/image_proc/rectify.hpp b/image_proc/include/image_proc/rectify.hpp index c1f9915b8..cd87f0649 100644 --- a/image_proc/include/image_proc/rectify.hpp +++ b/image_proc/include/image_proc/rectify.hpp @@ -34,6 +34,7 @@ #define IMAGE_PROC__RECTIFY_HPP_ #include +#include #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!) diff --git a/image_proc/include/image_proc/resize.hpp b/image_proc/include/image_proc/resize.hpp index d51bca4b9..7ec0eac4c 100644 --- a/image_proc/include/image_proc/resize.hpp +++ b/image_proc/include/image_proc/resize.hpp @@ -34,6 +34,7 @@ #define IMAGE_PROC__RESIZE_HPP_ #include +#include #include #include @@ -59,6 +60,7 @@ class ResizeNode double scale_width_; int height_; int width_; + std::string image_topic_; cv_bridge::CvImage scaled_cv_; diff --git a/image_proc/src/rectify.cpp b/image_proc/src/rectify.cpp index d814a3062..405d6b2fd 100644 --- a/image_proc/src/rectify.cpp +++ b/image_proc/src/rectify.cpp @@ -32,6 +32,7 @@ #include #include +#include #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("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 = diff --git a/image_proc/src/resize.cpp b/image_proc/src/resize.cpp index 628f09e64..d0122d26b 100644 --- a/image_proc/src/resize.cpp +++ b/image_proc/src/resize.cpp @@ -32,6 +32,7 @@ #include #include +#include #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("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(