From a7118b62a065c02a70266a2dda3e4b296ccad886 Mon Sep 17 00:00:00 2001
From: storrrrrrrrm <103425473+storrrrrrrrm@users.noreply.github.com>
Date: Mon, 9 May 2022 17:15:57 +0800
Subject: [PATCH] fix(image_projection_based_fusion): set imagebuffersize
 (#820)

* fix: set imagebuffersize configured

Signed-off-by: suchang <chang.su@autocore.ai>

* ci(pre-commit): autofix

Co-authored-by: suchang <chang.su@autocore.ai>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 .../include/image_projection_based_fusion/debugger.hpp     | 5 +++--
 .../launch/roi_cluster_fusion.launch.xml                   | 2 ++
 .../launch/roi_detected_object_fusion.launch.xml           | 2 ++
 perception/image_projection_based_fusion/src/debugger.cpp  | 7 +++++--
 .../image_projection_based_fusion/src/fusion_node.cpp      | 4 +++-
 5 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp
index ff790894658ec..9ecd336818b59 100644
--- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp
+++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp
@@ -38,7 +38,8 @@ using sensor_msgs::msg::RegionOfInterest;
 class Debugger
 {
 public:
-  explicit Debugger(rclcpp::Node * node_ptr, const std::size_t image_num);
+  explicit Debugger(
+    rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size);
 
   void publishImage(const std::size_t image_id, const rclcpp::Time & stamp);
 
@@ -57,7 +58,7 @@ class Debugger
   std::vector<image_transport::Subscriber> image_subs_;
   std::vector<image_transport::Publisher> image_pubs_;
   std::vector<boost::circular_buffer<sensor_msgs::msg::Image::ConstSharedPtr>> image_buffers_;
-  std::size_t image_buffer_size{5};
+  std::size_t image_buffer_size_;
 };
 
 }  // namespace image_projection_based_fusion
diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml
index 8fe94963ff8ca..e5824dfcff97f 100644
--- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml
+++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml
@@ -24,6 +24,7 @@
 
   <!-- debug -->
   <arg name="debug_mode" default="false"/>
+  <arg name="image_buffer_size" default="15"/>
   <arg name="input/image0" default="/image_raw0"/>
   <arg name="input/image1" default="/image_raw1"/>
   <arg name="input/image2" default="/image_raw2"/>
@@ -70,6 +71,7 @@
 
       <!-- debug -->
       <param name="debug_mode" value="$(var debug_mode)"/>
+      <param name="image_buffer_size" value="$(var image_buffer_size)"/>
     </node>
   </group>
 </launch>
diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml
index f6466e6512a09..be981478e78a2 100644
--- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml
+++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml
@@ -24,6 +24,7 @@
 
   <!-- debug -->
   <arg name="debug_mode" default="false"/>
+  <arg name="image_buffer_size" default="15"/>
   <arg name="input/image0" default="/image_raw0"/>
   <arg name="input/image1" default="/image_raw1"/>
   <arg name="input/image2" default="/image_raw2"/>
@@ -70,6 +71,7 @@
 
       <!-- debug -->
       <param name="debug_mode" value="$(var debug_mode)"/>
+      <param name="image_buffer_size" value="$(var image_buffer_size)"/>
     </node>
   </group>
 </launch>
diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp
index e1254fc136304..0bcfb019d93df 100644
--- a/perception/image_projection_based_fusion/src/debugger.cpp
+++ b/perception/image_projection_based_fusion/src/debugger.cpp
@@ -34,9 +34,12 @@ void drawRoiOnImage(
 
 namespace image_projection_based_fusion
 {
-Debugger::Debugger(rclcpp::Node * node_ptr, const std::size_t image_num) : node_ptr_(node_ptr)
+Debugger::Debugger(
+  rclcpp::Node * node_ptr, const std::size_t image_num, const std::size_t image_buffer_size)
+: node_ptr_(node_ptr)
 {
   image_buffers_.resize(image_num);
+  image_buffer_size_ = image_buffer_size;
   for (std::size_t img_i = 0; img_i < image_num; ++img_i) {
     auto sub = image_transport::create_subscription(
       node_ptr, "input/image_raw" + std::to_string(img_i),
@@ -54,7 +57,7 @@ Debugger::Debugger(rclcpp::Node * node_ptr, const std::size_t image_num) : node_
     auto pub =
       image_transport::create_publisher(node_ptr, "output/image_raw" + std::to_string(img_i));
     image_pubs_.push_back(pub);
-    image_buffers_.at(img_i).set_capacity(image_buffer_size);
+    image_buffers_.at(img_i).set_capacity(image_buffer_size_);
   }
 }
 
diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp
index 57ed245f67bd7..5d33303ca1bf9 100644
--- a/perception/image_projection_based_fusion/src/fusion_node.cpp
+++ b/perception/image_projection_based_fusion/src/fusion_node.cpp
@@ -127,7 +127,9 @@ FusionNode<Msg>::FusionNode(const std::string & node_name, const rclcpp::NodeOpt
 
   // debugger
   if (declare_parameter("debug_mode", false)) {
-    debugger_ = std::make_shared<Debugger>(this, rois_number_);
+    std::size_t image_buffer_size =
+      static_cast<std::size_t>(declare_parameter("image_buffer_size", 15));
+    debugger_ = std::make_shared<Debugger>(this, rois_number_, image_buffer_size);
   }
 }