From 257d4114801901877c531667f8aeb44d31077174 Mon Sep 17 00:00:00 2001
From: Michael Ferguson <mfergs7@gmail.com>
Date: Wed, 17 Jan 2024 21:45:01 -0500
Subject: [PATCH] same treatment, more nodes

---
 image_view/src/disparity_view_node.cpp | 10 +++++++---
 image_view/src/extract_images_node.cpp |  8 +++++---
 image_view/src/image_saver_node.cpp    |  6 ++++--
 image_view/src/stereo_view_node.cpp    |  7 ++++---
 image_view/src/video_recorder_node.cpp |  7 +++++--
 5 files changed, 25 insertions(+), 13 deletions(-)

diff --git a/image_view/src/disparity_view_node.cpp b/image_view/src/disparity_view_node.cpp
index 4bc04aaa9..be0805676 100644
--- a/image_view/src/disparity_view_node.cpp
+++ b/image_view/src/disparity_view_node.cpp
@@ -65,10 +65,14 @@ namespace image_view
 DisparityViewNode::DisparityViewNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("disparity_view_node", options)
 {
-  std::string topic =
-    rclcpp::expand_topic_or_service_name("image", this->get_name(), this->get_namespace());
+  // 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("image", false);
 
-  if (topic == "image") {
+  auto topics = this->get_topic_names_and_types();
+
+  if (topics.find(topic) == topics.end()) {
     RCLCPP_WARN(
       this->get_logger(), "Topic 'image' has not been remapped! Typical command-line usage:\n"
       "\t$ ros2 run image_view disparity_view --ros-args -r image:=<disparity image topic>");
diff --git a/image_view/src/extract_images_node.cpp b/image_view/src/extract_images_node.cpp
index f27f342d6..bf6513a97 100644
--- a/image_view/src/extract_images_node.cpp
+++ b/image_view/src/extract_images_node.cpp
@@ -70,8 +70,10 @@ ExtractImagesNode::ExtractImagesNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("extract_images_node", options),
   filename_format_(""), count_(0), _time(this->now())
 {
-  auto topic = rclcpp::expand_topic_or_service_name(
-    "image", this->get_name(), this->get_namespace());
+  // 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("image", false);
 
   this->declare_parameter<std::string>("transport", std::string("raw"));
   std::string transport = this->get_parameter("transport").as_string();
@@ -83,7 +85,7 @@ ExtractImagesNode::ExtractImagesNode(const rclcpp::NodeOptions & options)
 
   auto topics = this->get_topic_names_and_types();
 
-  if (topics.find(topic) != topics.end()) {
+  if (topics.find(topic) == topics.end()) {
     RCLCPP_WARN(
       this->get_logger(), "extract_images: image has not been remapped! "
       "Typical command-line usage:\n\t$ ros2 run image_view extract_images "
diff --git a/image_view/src/image_saver_node.cpp b/image_view/src/image_saver_node.cpp
index 302651880..5760b1488 100644
--- a/image_view/src/image_saver_node.cpp
+++ b/image_view/src/image_saver_node.cpp
@@ -70,8 +70,10 @@ namespace image_view
 ImageSaverNode::ImageSaverNode(const rclcpp::NodeOptions & options)
 : rclcpp::Node("image_saver_node", options)
 {
-  auto topic = rclcpp::expand_topic_or_service_name(
-    "image", this->get_name(), this->get_namespace());
+  // 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("image", false);
 
   // Useful when CameraInfo is being published
   cam_sub_ = image_transport::create_camera_subscription(
diff --git a/image_view/src/stereo_view_node.cpp b/image_view/src/stereo_view_node.cpp
index a9d21d018..04c015e62 100644
--- a/image_view/src/stereo_view_node.cpp
+++ b/image_view/src/stereo_view_node.cpp
@@ -109,9 +109,10 @@ StereoViewNode::StereoViewNode(const rclcpp::NodeOptions & options)
   cv::setMouseCallback("right", &StereoViewNode::mouseCb, this);
   cv::setMouseCallback("disparity", &StereoViewNode::mouseCb, this);
 
-  // Resolve topic names
-  std::string stereo_ns = rclcpp::expand_topic_or_service_name(
-    "stereo", this->get_name(), this->get_namespace());
+  // 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 stereo_ns = node_base->resolve_topic_or_service_name("stereo", false);
 
   std::string left_topic = rclcpp::expand_topic_or_service_name(
     stereo_ns + "/left" + rclcpp::expand_topic_or_service_name(
diff --git a/image_view/src/video_recorder_node.cpp b/image_view/src/video_recorder_node.cpp
index bed3ced59..3438d58a4 100644
--- a/image_view/src/video_recorder_node.cpp
+++ b/image_view/src/video_recorder_node.cpp
@@ -66,8 +66,11 @@ VideoRecorderNode::VideoRecorderNode(const rclcpp::NodeOptions & options)
     rclcpp::shutdown();
   }
 
-  auto topic = rclcpp::expand_topic_or_service_name(
-    "image", this->get_name(), this->get_namespace());
+  // 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("image", false);
+
   sub_image = image_transport::create_subscription(
     this, topic, std::bind(&VideoRecorderNode::callback, this, std::placeholders::_1), "raw");