From bb4ab63f19ec4d7df9cd80a0785ed2e4ffa144d1 Mon Sep 17 00:00:00 2001
From: quic-zhaoyuan <164289792+quic-zhaoyuan@users.noreply.github.com>
Date: Fri, 29 Nov 2024 04:02:45 +0800
Subject: [PATCH] stereo_image_proc: disparity_node: Add parameter to control
 camera_info (#1051)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Add a parameter, use_image_transport_camera_info (default:
true), to control whether DisparityNode uses
image_transport::getCameraInfoTopic for deriving camera_info topics.

    Default Behavior (backward compatible):
When use_image_transport_camera_info is true, the node continues using
image_transport::getCameraInfoTopic for camera_info resolution,
maintaining existing functionality.

    Custom Behavior:
When use_image_transport_camera_info is false, the node directly uses
the camera_info topics specified via remapping (e.g., left/camera_info
and right/camera_info), bypassing image_transport's derivation logic.

This solution allows users to explicitly remap the camera_info topics
for both cameras, providing flexibility for scenarios where topic names
are not unique or need customization.

---------

Signed-off-by: Zhaoyuan Cheng <quic_zhaochen@quicinc.com>
Signed-off-by: Zhaoyuan <quic_zhaochen@quicinc.com>
Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
---
 stereo_image_proc/doc/components.rst          |  3 +++
 .../src/stereo_image_proc/disparity_node.cpp  | 23 ++++++++++++++-----
 2 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/stereo_image_proc/doc/components.rst b/stereo_image_proc/doc/components.rst
index 1fab34c9c..51b482bb4 100644
--- a/stereo_image_proc/doc/components.rst
+++ b/stereo_image_proc/doc/components.rst
@@ -89,6 +89,9 @@ Parameters
    over the network than camera info and/or the delay from disparity processing
    is too long.
 
+*Common*
+ * **use_image_transport_camera_info** (bool, default: true): To control whether DisparityNode uses image_transport::getCameraInfoTopic for deriving camera_info topics. To set false, the node directly uses the camera_info topics specified via remapping (e.g., left/camera_info and right/camera_info), bypassing image_transport's derivation logic.
+
 stereo_image_proc::PointCloudNode
 ---------------------------------
 Combines a rectified color image and disparity image to produce a
diff --git a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp
index 8ccf60bca..da09a7d56 100644
--- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp
+++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp
@@ -73,6 +73,7 @@ class DisparityNode : public rclcpp::Node
     SEMI_GLOBAL_BLOCK_MATCHING
   };
 
+  bool use_image_transport_camera_info;
   // Subscriptions
   image_transport::SubscriberFilter sub_l_image_, sub_r_image_;
   message_filters::Subscriber<sensor_msgs::msg::CameraInfo> sub_l_info_, sub_r_info_;
@@ -170,6 +171,8 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
   bool approx = this->declare_parameter("approximate_sync", false);
   double approx_sync_epsilon = this->declare_parameter("approximate_sync_tolerance_seconds", 0.0);
   this->declare_parameter("use_system_default_qos", false);
+  use_image_transport_camera_info = this->declare_parameter("use_image_transport_camera_info",
+      true);
 
   // Synchronize callbacks
   if (approx) {
@@ -307,12 +310,20 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
         std::string right_topic =
           node_base->resolve_topic_or_service_name("right/image_rect", false);
         // Allow also remapping camera_info to something different than default
-        std::string left_info_topic =
-          node_base->resolve_topic_or_service_name(
-          image_transport::getCameraInfoTopic(left_topic), false);
-        std::string right_info_topic =
-          node_base->resolve_topic_or_service_name(
-          image_transport::getCameraInfoTopic(right_topic), false);
+        std::string left_info_topic;
+        std::string right_info_topic;
+
+        if (use_image_transport_camera_info) {
+          // Use image_transport to derive camera_info topics
+          left_info_topic = node_base->resolve_topic_or_service_name(
+            image_transport::getCameraInfoTopic(left_topic), false);
+          right_info_topic = node_base->resolve_topic_or_service_name(
+            image_transport::getCameraInfoTopic(right_topic), false);
+        } else {
+          // Use default camera_info topics
+          left_info_topic = node_base->resolve_topic_or_service_name("left/camera_info", false);
+          right_info_topic = node_base->resolve_topic_or_service_name("right/camera_info", false);
+        }
 
         // REP-2003 specifies that subscriber should be SensorDataQoS
         const auto sensor_data_qos = rclcpp::SensorDataQoS();