From 14f604a8c04c079339e43b2a5a1e3ec6dec94b29 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Mon, 12 Feb 2024 13:13:07 -0500 Subject: [PATCH] fix image publisher remapping (#941) Addresses #940 - fixes the compressed/etc topic remapping for publishers --- depth_image_proc/src/register.cpp | 8 +++++++- image_proc/src/crop_decimate.cpp | 3 ++- image_proc/src/crop_non_zero.cpp | 3 ++- image_proc/src/debayer.cpp | 9 ++++++--- image_proc/src/resize.cpp | 5 +++-- image_rotate/src/image_rotate_node.cpp | 9 +++++++-- 6 files changed, 27 insertions(+), 10 deletions(-) diff --git a/depth_image_proc/src/register.cpp b/depth_image_proc/src/register.cpp index b6cde396f..6269f4cf4 100644 --- a/depth_image_proc/src/register.cpp +++ b/depth_image_proc/src/register.cpp @@ -143,9 +143,15 @@ RegisterNode::RegisterNode(const rclcpp::NodeOptions & options) sub_rgb_info_.subscribe(this, "rgb/camera_info"); } }; + // 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(); + std::string topic = + node_base->resolve_topic_or_service_name("depth_registered/image_rect", false); + pub_registered_ = image_transport::create_camera_publisher( - this, "depth_registered/image_rect", + this, topic, rmw_qos_profile_default, pub_options); } diff --git a/image_proc/src/crop_decimate.cpp b/image_proc/src/crop_decimate.cpp index d439bb058..a53ab9ac2 100644 --- a/image_proc/src/crop_decimate.cpp +++ b/image_proc/src/crop_decimate.cpp @@ -115,6 +115,7 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options) // 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); + std::string pub_topic = node_base->resolve_topic_or_service_name("out/image_raw", false); queue_size_ = this->declare_parameter("queue_size", 5); target_frame_id_ = this->declare_parameter("target_frame_id", std::string()); @@ -156,7 +157,7 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options) // Create publisher with QoS matched to subscribed topic publisher auto qos_profile = getTopicQosProfile(this, image_topic_); - pub_ = image_transport::create_camera_publisher(this, "out/image_raw", qos_profile, pub_options); + pub_ = image_transport::create_camera_publisher(this, pub_topic, qos_profile, pub_options); } void CropDecimateNode::imageCb( diff --git a/image_proc/src/crop_non_zero.cpp b/image_proc/src/crop_non_zero.cpp index 488300e6d..100a23ed7 100644 --- a/image_proc/src/crop_non_zero.cpp +++ b/image_proc/src/crop_non_zero.cpp @@ -60,6 +60,7 @@ CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options) // 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); + std::string pub_topic = node_base->resolve_topic_or_service_name("image", false); // Setup lazy subscriber using publisher connection callback rclcpp::PublisherOptions pub_options; @@ -84,7 +85,7 @@ CropNonZeroNode::CropNonZeroNode(const rclcpp::NodeOptions & options) // Create publisher with QoS matched to subscribed topic publisher auto qos_profile = getTopicQosProfile(this, image_topic_); - pub_ = image_transport::create_publisher(this, "image", qos_profile, pub_options); + pub_ = image_transport::create_publisher(this, pub_topic, 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 7908a8c2c..fe4faf9ec 100644 --- a/image_proc/src/debayer.cpp +++ b/image_proc/src/debayer.cpp @@ -56,10 +56,14 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options) // TransportHints does not actually declare the parameter this->declare_parameter("image_transport", "raw"); + debayer_ = this->declare_parameter("debayer", 3); + // 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); + std::string mono_topic = node_base->resolve_topic_or_service_name("image_mono", false); + std::string color_topic = node_base->resolve_topic_or_service_name("image_color", false); // Setup lazy subscriber using publisher connection callback rclcpp::PublisherOptions pub_options; @@ -85,9 +89,8 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options) // Create publisher with QoS matched to subscribed topic publisher 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); + pub_mono_ = image_transport::create_publisher(this, mono_topic, qos_profile, pub_options); + pub_color_ = image_transport::create_publisher(this, color_topic, qos_profile, pub_options); } void DebayerNode::imageCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg) diff --git a/image_proc/src/resize.cpp b/image_proc/src/resize.cpp index a9b1d37ff..fdd908df1 100644 --- a/image_proc/src/resize.cpp +++ b/image_proc/src/resize.cpp @@ -59,6 +59,7 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options) // 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); + std::string pub_topic = node_base->resolve_topic_or_service_name("resize/image_raw", false); // Declare parameters before we setup any publishers or subscribers interpolation_ = this->declare_parameter("interpolation", 1); @@ -93,8 +94,8 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options) // Create publisher with QoS matched to subscribed topic publisher auto qos_profile = getTopicQosProfile(this, image_topic_); - pub_image_ = image_transport::create_camera_publisher( - this, "resize/image_raw", qos_profile, pub_options); + pub_image_ = + image_transport::create_camera_publisher(this, pub_topic, qos_profile, pub_options); } void ResizeNode::imageCb( diff --git a/image_rotate/src/image_rotate_node.cpp b/image_rotate/src/image_rotate_node.cpp index 85a596edb..280954489 100644 --- a/image_rotate/src/image_rotate_node.cpp +++ b/image_rotate/src/image_rotate_node.cpp @@ -338,8 +338,13 @@ void ImageRotateNode::onInit() } } }; - img_pub_ = image_transport::create_publisher( - this, "rotated/image", rmw_qos_profile_default, pub_options); + + // 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(); + std::string topic = node_base->resolve_topic_or_service_name("rotated/image", false); + + img_pub_ = image_transport::create_publisher(this, topic, rmw_qos_profile_default, pub_options); } } // namespace image_rotate