[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

@@ -133,8 +133,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;

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>
@@ -95,9 +93,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 +104,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>();