diff --git a/detector2d_base/include/detector2d_base/detector2d_base.hpp b/detector2d_base/include/detector2d_base/detector2d_base.hpp index 82d99aa..f892ed3 100644 --- a/detector2d_base/include/detector2d_base/detector2d_base.hpp +++ b/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() {} }; diff --git a/detector2d_node/include/detector2d_node/detector2d_node.hpp b/detector2d_node/include/detector2d_node/detector2d_node.hpp index 9305337..e3aef4d 100644 --- a/detector2d_node/include/detector2d_node/detector2d_node.hpp +++ b/detector2d_node/include/detector2d_node/detector2d_node.hpp @@ -21,6 +21,7 @@ #include #include #include +#include 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::SharedPtr image_sub_; @@ -38,6 +41,6 @@ class Detector2dNode : public rclcpp::Node std::shared_ptr param_listener_; - detector2d_parameters::Detector2dParam params_; + detector2d_parameters::Params params_; }; } diff --git a/detector2d_node/package.xml b/detector2d_node/package.xml index 97b1bb9..66b76e8 100644 --- a/detector2d_node/package.xml +++ b/detector2d_node/package.xml @@ -13,6 +13,7 @@ libopencv-dev pluginlib detector2d_plugins + detector2d_param rclcpp rclcpp_components sensor_msgs diff --git a/detector2d_node/src/detector2d_node.cpp b/detector2d_node/src/detector2d_node.cpp index cfa7d3d..0c22b3a 100644 --- a/detector2d_node/src/detector2d_node.cpp +++ b/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" diff --git a/detector2d_plugins/src/panel_simple_detector.cpp b/detector2d_plugins/src/panel_simple_detector.cpp index 66d52b8..c7879c6 100644 --- a/detector2d_plugins/src/panel_simple_detector.cpp +++ b/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); diff --git a/detector2d_plugins/src/publish_center_plugin.cpp b/detector2d_plugins/src/publish_center_plugin.cpp index 3ebd409..e835f55 100644 --- a/detector2d_plugins/src/publish_center_plugin.cpp +++ b/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; } }