fix image publisher remapping (ros-perception#941)

Addresses ros-perception#940 - fixes the compressed/etc topic remapping for publishers

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);
 }
 

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(

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)

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<std::string>("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)

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(

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