From e6dacd6eaad658be16a4bde7e83bd5e399d0a51f Mon Sep 17 00:00:00 2001
From: SamerKhshiboun <samer.khshiboun@intel.com>
Date: Sun, 2 Jul 2023 12:54:57 +0300
Subject: [PATCH] Fix overriding frames on same topics/CVimages due to a bug in
 PR2759

---
 .../include/base_realsense_node.h             | 11 ++++++---
 realsense2_camera/src/base_realsense_node.cpp | 23 +++++++++++--------
 2 files changed, 21 insertions(+), 13 deletions(-)

diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h
index 76d7bfe032..bac935135d 100755
--- a/realsense2_camera/include/base_realsense_node.h
+++ b/realsense2_camera/include/base_realsense_node.h
@@ -206,9 +206,14 @@ namespace realsense2_camera
 
         IMUInfo getImuInfo(const rs2::stream_profile& profile);
         
-        void publishFrame(rs2::frame f, const rclcpp::Time& t,
-                          const stream_index_pair& stream,
-                          const bool is_publishMetadata = true);
+        void publishFrame(rs2::frame f,
+            const rclcpp::Time& t,
+            const stream_index_pair& stream,
+            std::map<stream_index_pair, cv::Mat>& images,
+            const std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>& info_publishers,
+            const std::map<stream_index_pair, std::shared_ptr<image_publisher>>& image_publishers,
+            const bool is_publishMetadata = true);
+
         void publishMetadata(rs2::frame f, const rclcpp::Time& header_time, const std::string& frame_id);
 
         sensor_msgs::msg::Imu CreateUnitedMessage(const CimuData accel_data, const CimuData gyro_data);
diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp
index 1ffe674d45..8101178888 100755
--- a/realsense2_camera/src/base_realsense_node.cpp
+++ b/realsense2_camera/src/base_realsense_node.cpp
@@ -534,11 +534,11 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
                 sent_depth_frame = true;
                 if (original_color_frame && _align_depth_filter->is_enabled())
                 {
-                    publishFrame(f, t, COLOR, false);
+                    publishFrame(f, t, COLOR, _depth_aligned_image, _depth_aligned_info_publisher, _depth_aligned_image_publishers, false);
                     continue;
                 }
             }
-            publishFrame(f, t, sip);
+            publishFrame(f, t, sip, _images, _info_publishers, _image_publishers);
         }
         if (original_depth_frame && _align_depth_filter->is_enabled())
         {
@@ -548,7 +548,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
             else
                 frame_to_send = original_depth_frame;
                 
-            publishFrame(frame_to_send, t, DEPTH);
+            publishFrame(frame_to_send, t, DEPTH, _images, _info_publishers, _image_publishers);
         }
     }
     else if (frame.is<rs2::video_frame>())
@@ -566,8 +566,8 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame)
                 clip_depth(frame, _clipping_distance);
             }
         }
-        publishFrame(frame, t, sip);
-    }
+        publishFrame(frame, t, sip, _images, _info_publishers, _image_publishers);
+     }
     _synced_imu_publisher->Resume();
 } // frame_callback
 
@@ -820,6 +820,9 @@ IMUInfo BaseRealSenseNode::getImuInfo(const rs2::stream_profile& profile)
 
 void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t,
                                      const stream_index_pair& stream,
+                                     std::map<stream_index_pair, cv::Mat>& images,
+                                     const std::map<stream_index_pair, rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr>& info_publishers,
+                                     const std::map<stream_index_pair, std::shared_ptr<image_publisher>>& image_publishers,
                                      const bool is_publishMetadata)
 {
     ROS_DEBUG("publishFrame(...)");
@@ -833,7 +836,7 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t,
         height = timage.get_height();
         bpp = timage.get_bytes_per_pixel();
     }
-    auto& image = _images[stream];
+    auto& image = images[stream];
 
     if (image.size() != cv::Size(width, height) || image.depth() != _image_formats[bpp])
     {
@@ -846,14 +849,14 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t,
         image = fix_depth_scale(image, _depth_scaled_image[stream]);
     }
 
-    if (_info_publishers.find(stream) == _info_publishers.end() ||
-        _image_publishers.find(stream) == _image_publishers.end())
+    if (info_publishers.find(stream) == info_publishers.end() ||
+        image_publishers.find(stream) == image_publishers.end())
         {
             // Stream is already disabled.
             return;
         }
-    auto& info_publisher = _info_publishers.at(stream);
-    auto& image_publisher = _image_publishers.at(stream);
+    auto& info_publisher = info_publishers.at(stream);
+    auto& image_publisher = image_publishers.at(stream);
     if(0 != info_publisher->get_subscription_count() ||
        0 != image_publisher->get_subscription_count())
     {