[Enhancement] - Receive Frame Mat from Subscriber

CMakeLists.txt

@@ -19,7 +19,8 @@ find_package(ninshiki_interfaces REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(shisen_cpp REQUIRED)
-find_package(shisen_interfaces REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(cv_bridge REQUIRED)
 
 add_library(${PROJECT_NAME} SHARED
   "src/${PROJECT_NAME}/detector/color_detector.cpp"
@@ -41,7 +42,8 @@ ament_target_dependencies(${PROJECT_NAME}
   OpenCV
   rclcpp
   shisen_cpp
-  shisen_interfaces)
+  sensor_msgs
+  cv_bridge)
 
 install(DIRECTORY "include" DESTINATION ".")
 
@@ -72,7 +74,8 @@ ament_export_dependencies(
   OpenCV
   rclcpp
   shisen_cpp
-  shisen_interfaces)
+  sensor_msgs
+  cv_bridge)
 ament_export_include_directories("include")
 ament_export_libraries(${PROJECT_NAME})
 ament_package()

include/ninshiki_cpp/node/ninshiki_cpp_node.hpp

@@ -29,12 +29,12 @@
 #include <string>
 
 #include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/image.hpp"
 #include "ninshiki_cpp/detector/color_detector.hpp"
 #include "ninshiki_cpp/detector/detector.hpp"
 #include "ninshiki_cpp/detector/dnn_detector.hpp"
 #include "ninshiki_cpp/detector/lbp_detector.hpp"
 #include "ninshiki_interfaces/msg/detected_objects.hpp"
-#include "shisen_interfaces/msg/image.hpp"
 #include "shisen_cpp/shisen_cpp.hpp"
 
 namespace ninshiki_cpp::node
@@ -48,7 +48,7 @@ class NinshikiCppNode
   using LBPDetector = ninshiki_cpp::detector::LBPDetector;
 
   NinshikiCppNode(
-    rclcpp::Node::SharedPtr node, std::string topic_name,
+    rclcpp::Node::SharedPtr node,
     int frequency, shisen_cpp::Options options);
   void publish();
   void set_detection(
@@ -59,18 +59,18 @@ class NinshikiCppNode
 private:
   using Contours = ninshiki_interfaces::msg::Contours;
   using DetectedObjects = ninshiki_interfaces::msg::DetectedObjects;
-  using Image = shisen_interfaces::msg::Image;
+  using Image = sensor_msgs::msg::Image;
 
   rclcpp::Node::SharedPtr node;
   rclcpp::TimerBase::SharedPtr node_timer;
 
   rclcpp::Publisher<DetectedObjects>::SharedPtr detected_object_publisher;
   rclcpp::Publisher<Contours>::SharedPtr field_segmentation_publisher;
+  rclcpp::Subscription<Image>::SharedPtr image_subscriber;
 
   std::shared_ptr<DnnDetector> dnn_detection;
   std::shared_ptr<ColorDetector> color_detection;
   std::shared_ptr<LBPDetector> lbp_detection;
-  std::shared_ptr<shisen_cpp::camera::ImageProvider> image_provider;
 
   cv::Mat received_frame;
   cv::Mat hsv_frame;

package.xml

@@ -13,7 +13,8 @@
   <depend>nlohmann-json-dev</depend>
   <depend>rclcpp</depend>
   <depend>shisen_cpp</depend>
-  <depend>shisen_interfaces</depend>
+  <depend>sensor_msgs</depend>
+  <depend>cv_bridge</depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
   <export>

src/ninshiki_cpp/detector/color_detector.cpp

@@ -99,9 +99,10 @@ bool ColorDetector::save_configuration()
     int max_hsv[] = {item.max_hue, item.max_saturation, item.max_value};
 
     nlohmann::json color = {
-      {"name", item.name},
-      {"min_hsv", min_hsv},
-      {"max_hsv", max_hsv},
+      {item.name, {
+        {"min_hsv", min_hsv},
+        {"max_hsv", max_hsv}
+      }}
     };
 
     config.push_back(color);
@@ -133,8 +134,8 @@ bool ColorDetector::sync_configuration()
 
 cv::Mat ColorDetector::classify(cv::Mat input)
 {
-  int h_min = (min_hue * 180) / 360;
-  int h_max = (max_hue * 180) / 360;
+  int h_min = (min_hue * 255) / 360;
+  int h_max = (max_hue * 255) / 360;
 
   int s_min = (min_saturation * 255) / 100;
   int s_max = (max_saturation * 255) / 100;
@@ -189,10 +190,6 @@ void ColorDetector::find(cv::Mat binary_mat)
 
 void ColorDetector::detection(const cv::Mat & image)
 {
-  // Get width and height from image
-  double img_width = static_cast<double>(image.cols);
-  double img_height = static_cast<double>(image.rows);
-
   // iterate every color in colors
   for (auto & color : colors) {
     color_name = color.name;
@@ -213,8 +210,8 @@ void ColorDetector::detection(const cv::Mat & image)
 
         for (cv::Point & point : contour) {
           ninshiki_interfaces::msg::Point point_msg;
-          point_msg.x = static_cast<float>(point.x) / img_width;
-          point_msg.y = static_cast<float>(point.y) / img_height;
+          point_msg.x = static_cast<float>(point.x);
+          point_msg.y = static_cast<float>(point.y);
 
           contour_msg.name = color_name;
           contour_msg.contour.push_back(point_msg);

src/ninshiki_cpp/detector/dnn_detector.cpp

@@ -55,7 +55,7 @@ DnnDetector::DnnDetector(bool gpu, bool myriad)
     net.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV);
     net.setPreferableTarget(cv::dnn::DNN_TARGET_MYRIAD);
   } else {
-    net.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV);
+    net.setPreferableBackend(cv::dnn::DNN_BACKEND_INFERENCE_ENGINE);
     net.setPreferableTarget(cv::dnn::DNN_TARGET_CPU);
   }
 }
@@ -75,8 +75,8 @@ void DnnDetector::detect_darknet(const cv::Mat & image, float conf_threshold, fl
 
   // Create a 4D blob from a frame
   static cv::Mat blob;
-  cv::Size input_size = cv::Size(416, 416);
-  cv::dnn::blobFromImage(image, blob, 1.0, input_size, cv::Scalar(), true, false, CV_8U);
+  cv::Size input_size = cv::Size(320, 320);
+  cv::dnn::blobFromImage(image, blob, 1.0, input_size, cv::Scalar(), false, false, CV_8U);
 
   net.setInput(blob, "", 0.00392, cv::Scalar(0, 0, 0, 0));
   std::vector<cv::Mat> outs;
@@ -164,10 +164,10 @@ void DnnDetector::detect_darknet(const cv::Mat & image, float conf_threshold, fl
         ninshiki_interfaces::msg::DetectedObject detection_object;
         detection_object.label = classes[class_ids[i]];
         detection_object.score = confidences[i];
-        detection_object.left = box.x / img_width;
-        detection_object.top = box.y / img_height;
-        detection_object.right = (box.x + box.width) / img_width;
-        detection_object.bottom = (box.y + box.height) / img_height;
+        detection_object.left = box.x;
+        detection_object.top = box.y;
+        detection_object.right = box.width;
+        detection_object.bottom = box.height;
 
         detection_result.detected_objects.push_back(detection_object);
       }

src/ninshiki_cpp/node/ninshiki_cpp_node.cpp

@@ -21,6 +21,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <cv_bridge/cv_bridge.hpp>
 #include "ninshiki_cpp/node/ninshiki_cpp_node.hpp"
 
 using namespace std::chrono_literals;
@@ -29,23 +30,27 @@ namespace ninshiki_cpp::node
 {
 
 NinshikiCppNode::NinshikiCppNode(
-  rclcpp::Node::SharedPtr node, std::string topic_name,
+  rclcpp::Node::SharedPtr node,
   int frequency, shisen_cpp::Options options)
-: node(node), dnn_detection(nullptr), color_detection(nullptr)
+: node(node), dnn_detection(nullptr), color_detection(nullptr), lbp_detection(nullptr)
 {
   detected_object_publisher = node->create_publisher<DetectedObjects>(
     get_node_prefix() + "/dnn_detection", 10);
   field_segmentation_publisher = node->create_publisher<Contours>(
     get_node_prefix() + "/color_detection", 10);
 
-  image_provider = std::make_shared<shisen_cpp::camera::ImageProvider>(options);
+  image_subscriber =
+    node->create_subscription<Image>("camera/image", 10, [this](const Image::SharedPtr message) {
+      if (!message->data.empty()) {
+        received_frame = cv_bridge::toCvShare(message, "bgr8")->image;
+      }
+    });
 
   node_timer = node->create_wall_timer(
     std::chrono::milliseconds(frequency),
     [this]() {
-      image_provider->update_mat();
-      received_frame = image_provider->get_mat();
       if (!received_frame.empty()) {
+        cv::cvtColor(received_frame, hsv_frame, cv::COLOR_BGR2HSV);
         publish();
       }
     }
@@ -64,7 +69,7 @@ void NinshikiCppNode::publish()
   detected_object_publisher->publish(lbp_detection->detection_result);
 
   // Clear detection_result
-  // received_frame.release();
+  received_frame.release();
   dnn_detection->detection_result.detected_objects.clear();
   color_detection->detection_result.contours.clear();
   lbp_detection->detection_result.detected_objects.clear();

src/ninshiki_cpp_detector.cpp

@@ -20,8 +20,6 @@
 
 #include <ninshiki_cpp/ninshiki_cpp.hpp>
 #include <rclcpp/rclcpp.hpp>
-#include <shisen_cpp/camera/node/camera_node.hpp>
-#include <shisen_cpp/camera/provider/image_provider.hpp>
 
 #include <memory>
 #include <string>
@@ -41,10 +39,11 @@ int main(int argc, char ** argv)
 
   const char * help_message =
     "Usage: ros2 run ninshiki_cpp detector\n"
-    "[topic] [--detector DETECTOR] [--GPU {0,1}] [--MYRIAD {0,1}]\n"
+    "[path] [--detector DETECTOR] [--GPU {0,1}] [--MYRIAD {0,1}]\n"
     "\n"
+    ""
     "Positional arguments:\n"
-    "topic                specify topic name to subscribe\n"
+    "path                 path to detection configuration\n"
     "\n"
     "Optional arguments:\n"
     "-h, --help           show this help message and exit\n"
@@ -55,6 +54,10 @@ int main(int argc, char ** argv)
 
   // Handle arguments
   try {
+    if (argc < 2) {
+      std::cerr << "Argument needed!\n\n" << help_message << std::endl;
+      return 1;
+    }
     int i = 1;
     int pos = 0;
     while (i < argc) {
@@ -95,9 +98,6 @@ int main(int argc, char ** argv)
           return 1;
         }
       } else if (pos == 0) {
-        topic_name = arg;
-        ++pos;
-      } else if (pos == 1) {
         path = arg;
         ++pos;
       }
@@ -109,7 +109,7 @@ int main(int argc, char ** argv)
 
   auto node = std::make_shared<rclcpp::Node>("ninshiki_cpp");
   auto ninshiki_cpp_node = std::make_shared<ninshiki_cpp::node::NinshikiCppNode>(
-    node, topic_name, frequency, options);
+    node, frequency, options);
 
   auto dnn_detection = std::make_shared<ninshiki_cpp::detector::DnnDetector>(gpu, myriad);
   auto color_detection = std::make_shared<ninshiki_cpp::detector::ColorDetector>();