Merge pull request #6 from StrayedCats/fix/detector2d_base

update detector2d

detector2d_base/include/detector2d_base/detector2d_base.hpp

@@ -27,33 +27,6 @@ class Detector
   virtual void init(const detector2d_parameters::ParamListener & param_listener) = 0;
   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)
-  {
-      cv::Mat3b frame_out;
-      if (frame.channels() == 1)
-      {
-          cv::cvtColor(frame, frame_out, cv::COLOR_GRAY2BGR);
-      }
-      else {
-          frame_out = frame;
-      }
-
-      for (auto detection : boxes.detections)
-      {
-          cv::Rect bbox;
-          bbox.x = detection.bbox.center.position.x - detection.bbox.size_x / 2;
-          bbox.y = detection.bbox.center.position.y - detection.bbox.size_y / 2;
-          bbox.width = detection.bbox.size_x;
-          bbox.height = detection.bbox.size_y;
-          cv::rectangle(frame_out, bbox, cv::Scalar(0, 255, 0), 2);
-      }
-      return frame_out;
-  }
-
 protected:
   Detector() {}
 };

detector2d_node/include/detector2d_node/detector2d_node.hpp

@@ -21,6 +21,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <vision_msgs/msg/detection2_d_array.hpp>
+#include <detector2d_param/detector2d_param.hpp>
 
 namespace detector2d_node
 {
@@ -29,6 +30,8 @@ class Detector2dNode : public rclcpp::Node
 public:
   Detector2dNode(const rclcpp::NodeOptions &);
   void image_callback(const sensor_msgs::msg::Image::SharedPtr msg);
+private:
+  cv::Mat3b draw_bboxes(const cv::Mat &, const vision_msgs::msg::Detection2DArray &);
 
 private:
   rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
@@ -38,6 +41,6 @@ class Detector2dNode : public rclcpp::Node
 
   std::shared_ptr<detector2d_parameters::ParamListener> param_listener_;
 
-  detector2d_parameters::Detector2dParam params_;
+  detector2d_parameters::Params params_;
 };
 }

detector2d_node/package.xml

@@ -13,6 +13,7 @@
   <depend>libopencv-dev</depend>
   <depend>pluginlib</depend>
   <depend>detector2d_plugins</depend>
+  <depend>detector2d_param</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>

detector2d_node/src/detector2d_node.cpp

@@ -27,9 +27,9 @@ Detector2dNode::Detector2dNode(const rclcpp::NodeOptions & options)
 
   try {
     this->detector_ = this->detection_loader_.createSharedInstance(
-      params.load_target_plugin);
+      this->params_.load_target_plugin);
     this->detector_->init(*this->param_listener_);
-    std::cout << "params.load_target_plugin: " << params.load_target_plugin << std::endl;
+    std::cout << "params.load_target_plugin: " << this->params_.load_target_plugin << std::endl;
   } catch (pluginlib::PluginlibException & ex) {
     RCLCPP_ERROR(this->get_logger(), "The plugin failed to load for some reason. Error: %s", ex.what());
   }
@@ -44,24 +44,52 @@ void Detector2dNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg
 {
   try {
     cv::Mat image = cv_bridge::toCvShare(msg, "bgr8")->image;
+
+    vision_msgs::msg::Detection2DArray bboxes =
+      this->detector_->detect(cv_bridge::toCvShare(msg, "bgr8")->image);
+    bboxes.header = msg->header;
+
+    RCLCPP_INFO(this->get_logger(), "Detected %ld objects", bboxes.detections.size());
+
+    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);
   } catch (std::exception & e) {
     RCLCPP_ERROR(this->get_logger(), "exception: %s", e.what());
     return;
   }
+}
 
-  vision_msgs::msg::Detection2DArray bboxes =
-    this->detector_->detect(cv_bridge::toCvShare(msg, "bgr8")->image);
-  bboxes.header = msg->header;
+cv::Mat3b Detector2dNode::draw_bboxes(
+    const cv::Mat & frame,
+    const vision_msgs::msg::Detection2DArray & boxes)
+{
+    cv::Mat3b frame_out;
+    if (frame.channels() == 1)
+    {
+        cv::cvtColor(frame, frame_out, cv::COLOR_GRAY2BGR);
+    }
+    else {
+        frame_out = frame;
+    }
 
-  if (this->params_.debug) {
-    cv::imshow("detector", this->draw_bboxes(image, bboxes));
-    auto key = cv::waitKey(1);
-    if (key == 27) {
-      rclcpp::shutdown();
+    for (auto detection : boxes.detections)
+    {
+        cv::Rect bbox;
+        bbox.x = detection.bbox.center.position.x - detection.bbox.size_x / 2;
+        bbox.y = detection.bbox.center.position.y - detection.bbox.size_y / 2;
+        bbox.width = detection.bbox.size_x;
+        bbox.height = detection.bbox.size_y;
+        cv::rectangle(frame_out, bbox, cv::Scalar(0, 255, 0), 2);
     }
-  }
-  this->pose_pub_->publish(bboxes);
+    return frame_out;
 }
+
 } // namespace detector2d_node
 
 #include "rclcpp_components/register_node_macro.hpp"

detector2d_plugins/src/panel_simple_detector.cpp

@@ -25,7 +25,6 @@ void PanelSimpleDetector::init(const detector2d_parameters::ParamListener & para
 Detection2DArray PanelSimpleDetector::detect(const cv::Mat & image)
 {
   auto panel_color_ = this->params_.panel_simple_detector.panel_color;
-  auto debug = this->params_.debug;
 
   cv::Mat3b hsv;
   cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV);

detector2d_plugins/src/publish_center_plugin.cpp

@@ -30,6 +30,8 @@ Detection2DArray PublishCenter::detect(const cv::Mat & image)
   pose.detections.resize(1);
   pose.detections[0].bbox.center.position.x = col / 2;
   pose.detections[0].bbox.center.position.y = row / 2;
+  pose.detections[0].bbox.size_x = 10;
+  pose.detections[0].bbox.size_y = 10;
   return pose;
 }
 }