From 3feac989b4dc75b36c8f91d4a8c80d87fd5ae432 Mon Sep 17 00:00:00 2001
From: Ar-Ray-code <ray255ar@gmail.com>
Date: Sat, 30 Mar 2024 22:37:33 +0900
Subject: [PATCH] update detector2d_base

---
 .../include/detector2d_base/detector2d_base.hpp  |  1 +
 .../include/detector2d_node/detector2d_node.hpp  |  2 ++
 detector2d_node/src/detector2d_node.cpp          | 10 +++++++++-
 .../detector2d_plugins/panel_simple_detector.hpp |  1 -
 detector2d_plugins/src/panel_simple_detector.cpp | 16 ----------------
 5 files changed, 12 insertions(+), 18 deletions(-)

diff --git a/detector2d_base/include/detector2d_base/detector2d_base.hpp b/detector2d_base/include/detector2d_base/detector2d_base.hpp
index a6a56b7..82d99aa 100644
--- a/detector2d_base/include/detector2d_base/detector2d_base.hpp
+++ b/detector2d_base/include/detector2d_base/detector2d_base.hpp
@@ -28,6 +28,7 @@ class Detector
   virtual vision_msgs::msg::Detection2DArray detect(const cv::Mat & image) = 0;
   virtual ~Detector() {}
 
+private:
   cv::Mat3b draw_bboxes(
       const cv::Mat & frame,
       const vision_msgs::msg::Detection2DArray & boxes)
diff --git a/detector2d_node/include/detector2d_node/detector2d_node.hpp b/detector2d_node/include/detector2d_node/detector2d_node.hpp
index f5fa4f9..9305337 100644
--- a/detector2d_node/include/detector2d_node/detector2d_node.hpp
+++ b/detector2d_node/include/detector2d_node/detector2d_node.hpp
@@ -37,5 +37,7 @@ class Detector2dNode : public rclcpp::Node
   std::shared_ptr<detector2d_base::Detector> detector_;
 
   std::shared_ptr<detector2d_parameters::ParamListener> param_listener_;
+
+  detector2d_parameters::Detector2dParam params_;
 };
 }
diff --git a/detector2d_node/src/detector2d_node.cpp b/detector2d_node/src/detector2d_node.cpp
index 202aff2..cfa7d3d 100644
--- a/detector2d_node/src/detector2d_node.cpp
+++ b/detector2d_node/src/detector2d_node.cpp
@@ -23,7 +23,7 @@ Detector2dNode::Detector2dNode(const rclcpp::NodeOptions & options)
 {
   this->param_listener_ = std::make_shared<detector2d_parameters::ParamListener>(
     this->get_node_parameters_interface());
-  const auto params = this->param_listener_->get_params();
+  this->params_ = this->param_listener_->get_params();
 
   try {
     this->detector_ = this->detection_loader_.createSharedInstance(
@@ -52,6 +52,14 @@ void Detector2dNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg
   vision_msgs::msg::Detection2DArray bboxes =
     this->detector_->detect(cv_bridge::toCvShare(msg, "bgr8")->image);
   bboxes.header = msg->header;
+
+  if (this->params_.debug) {
+    cv::imshow("detector", this->draw_bboxes(image, bboxes));
+    auto key = cv::waitKey(1);
+    if (key == 27) {
+      rclcpp::shutdown();
+    }
+  }
   this->pose_pub_->publish(bboxes);
 }
 } // namespace detector2d_node
diff --git a/detector2d_plugins/include/detector2d_plugins/panel_simple_detector.hpp b/detector2d_plugins/include/detector2d_plugins/panel_simple_detector.hpp
index 3c6a7ec..6ded697 100644
--- a/detector2d_plugins/include/detector2d_plugins/panel_simple_detector.hpp
+++ b/detector2d_plugins/include/detector2d_plugins/panel_simple_detector.hpp
@@ -33,7 +33,6 @@ class PanelSimpleDetector : public detector2d_base::Detector
   Detection2DArray detect(const cv::Mat &) override;
 
 private:
-  void draw(const cv::Mat &, const CenterPoints &, const std::string &);
   CenterPoints get_center_points(const Contours &);
   CenterPoints merge_center_points(const CenterPoints &, const int &);
 
diff --git a/detector2d_plugins/src/panel_simple_detector.cpp b/detector2d_plugins/src/panel_simple_detector.cpp
index cce626d..66d52b8 100644
--- a/detector2d_plugins/src/panel_simple_detector.cpp
+++ b/detector2d_plugins/src/panel_simple_detector.cpp
@@ -53,10 +53,6 @@ Detection2DArray PanelSimpleDetector::detect(const cv::Mat & image)
   CenterPoints center_points = get_center_points(contours);
   CenterPoints merged_center_points = merge_center_points(center_points, image.cols);
 
-  if (debug) {
-    draw(image, merged_center_points, "merged_center_points");
-  }
-
   Detection2DArray pose;
   for (auto center_point : merged_center_points) {
     vision_msgs::msg::Detection2D detection;
@@ -70,18 +66,6 @@ Detection2DArray PanelSimpleDetector::detect(const cv::Mat & image)
   return pose;
 }
 
-void PanelSimpleDetector::draw(
-  const cv::Mat & image, const CenterPoints & center_points,
-  const std::string & window_name)
-{
-  cv::Mat3b canvas = image.clone();
-  for (auto center_point : center_points) {
-    cv::circle(canvas, center_point, 2, cv::Scalar(0, 255, 0), 2);
-  }
-  cv::imshow(window_name, canvas);
-  cv::waitKey(1);
-}
-
 CenterPoints PanelSimpleDetector::get_center_points(const Contours & contours)
 {
   CenterPoints center_points;