From 201b75ff9342528daf198e54529c7b3bfc2962b0 Mon Sep 17 00:00:00 2001 From: Zhaoyuan Date: Wed, 27 Nov 2024 15:24:32 +0800 Subject: [PATCH 1/3] stereo_image_proc: disparity_node: Add parameter to control camera_info topic resolution Added `use_image_transport_camera_info` parameter to toggle between deriving camera_info topics using `image_transport::getCameraInfoTopic` or using default topic names. - Default behavior (`true`) maintains current logic for backward compatibility. - New option (`false`) supports direct usage of custom camera_info topics. Improves flexibility for custom remapping setups. Signed-off-by: Zhaoyuan Cheng --- .../src/stereo_image_proc/disparity_node.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) 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..ed6887026 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 sub_l_info_, sub_r_info_; @@ -170,6 +171,7 @@ 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 +309,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(); From 04e6a59a5a63d34f65278af41bd72e0b864ccdb6 Mon Sep 17 00:00:00 2001 From: Zhaoyuan Date: Thu, 28 Nov 2024 14:26:29 +0800 Subject: [PATCH 2/3] stereo_image_proc: update component.rsc to disprarity node add parameter use_image_transport_camera_info --- stereo_image_proc/doc/components.rst | 3 +++ 1 file changed, 3 insertions(+) diff --git a/stereo_image_proc/doc/components.rst b/stereo_image_proc/doc/components.rst index 8fe3ac66a..7d01c0b7c 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 From 647fd034a0faca723743c7f31ae054b9f81a89ff Mon Sep 17 00:00:00 2001 From: Zhaoyuan Date: Thu, 28 Nov 2024 14:57:20 +0800 Subject: [PATCH 3/3] stereo_image_proc: disparity_node: update code format Signed-off-by: Zhaoyuan --- stereo_image_proc/src/stereo_image_proc/disparity_node.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 ed6887026..da09a7d56 100644 --- a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp +++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp @@ -171,7 +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); + use_image_transport_camera_info = this->declare_parameter("use_image_transport_camera_info", + true); // Synchronize callbacks if (approx) {