Merge pull request #29 from ichiro-its/feature/create-record-image-service

[Feature] Create Record Image Service

Author: mbsaloka

CMakeLists.txt

@@ -40,6 +40,7 @@ add_library(${PROJECT_NAME} SHARED
   "src/${PROJECT_NAME}/config/grpc/call_data_get_image.cpp"
   "src/${PROJECT_NAME}/config/grpc/call_data_save_capture_setting.cpp"
   "src/${PROJECT_NAME}/config/grpc/call_data_set_capture_setting.cpp"
+  "src/${PROJECT_NAME}/config/grpc/call_data_record_image.cpp"
   "src/${PROJECT_NAME}/utility/capture_setting.cpp"
   "src/${PROJECT_NAME}/utility/interface.cpp"
   "src/${PROJECT_NAME}/node/shisen_cpp_node.cpp"

include/shisen_cpp/camera/node/camera_node.hpp

@@ -43,6 +43,7 @@ class CameraNode
   void update();
   void on_mat_captured(cv::Mat mat);
   void on_camera_config(int width, int height);
+  void save_image(cv::Mat mat);
   CaptureSetting on_configure_capture_setting(const CaptureSetting & capture_setting);
   void configure_capture_setting(const CaptureSetting & capture_setting = CaptureSetting());
   void load_configuration(const std::string & path);

include/shisen_cpp/config/grpc/call_data_record_image.hpp

@@ -0,0 +1,45 @@
+// Copyright (c) 2024 ICHIRO ITS
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#ifndef SHISEN_CPP__CONFIG__GRPC__CALL_DATA_RECORD_IMAGE_HPP__
+#define SHISEN_CPP__CONFIG__GRPC__CALL_DATA_RECORD_IMAGE_HPP__
+
+#include <shisen_cpp/camera/node/camera_node.hpp>
+#include <shisen_cpp/config/grpc/call_data.hpp>
+
+namespace shisen_cpp
+{
+class CallDataRecordImage
+: CallData<shisen_interfaces::proto::Empty, shisen_interfaces::proto::Empty>
+{
+public:
+  CallDataRecordImage(
+    shisen_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq,
+    const std::string & path, const std::shared_ptr<camera::CameraNode>& camera_node);
+
+protected:
+  void AddNextToCompletionQueue() override;
+  void WaitForRequest() override;
+  void HandleRequest() override;
+  std::shared_ptr<camera::CameraNode> camera_node_;
+};
+}  // namespace shisen_cpp
+
+#endif  // SHISEN_CPP__CONFIG__GRPC__CALL_DATA_RECORD_IMAGE_HPP__

src/shisen_cpp/camera/node/camera_node.cpp

@@ -18,16 +18,16 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 // THE SOFTWARE.
 
-#include <fstream>
-#include <nlohmann/json.hpp>
-#include <fstream>
-#include <memory>
+
 #include <nlohmann/json.hpp>
+#include <opencv2/imgcodecs.hpp>
 #include <shisen_cpp/camera/node/camera_node.hpp>
 #include <jitsuyo/config.hpp>
 
+#include <chrono>
 #include <fstream>
 #include <memory>
+#include <sstream>
 #include <string>
 
 namespace shisen_cpp::camera
@@ -69,6 +69,32 @@ void CameraNode::on_camera_config(int width, int height)
   camera_config_publisher->publish(camera_config_provider->get_camera_config());
 }
 
+void CameraNode::save_image(cv::Mat mat)
+{
+  if (!std::filesystem::exists("image")) {
+    if (!std::filesystem::create_directory("image")) {
+      RCLCPP_ERROR(node->get_logger(), "Error creating `image` directory!");
+      return;
+    }
+  }
+
+  auto now = std::chrono::system_clock::now();
+  std::time_t now_time_t = std::chrono::system_clock::to_time_t(now);
+  auto now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
+
+  std::stringstream ss;
+  ss << std::put_time(std::localtime(&now_time_t), "%Y-%m-%d_%H-%M-%S");
+  ss << '-' << std::setfill('0') << std::setw(3) << now_ms.count();
+  std::string timestamp = ss.str();
+
+  std::string filename = "image/" + timestamp + ".jpg";
+
+  bool result = cv::imwrite(filename, mat);
+  if (!result) {
+    RCLCPP_ERROR(node->get_logger(), "Failed to save image!");
+  }
+}
+
 cv::Mat CameraNode::get_mat()
 {
   update();

src/shisen_cpp/config/grpc/call_data_record_image.cpp

@@ -0,0 +1,54 @@
+// Copyright (c) 2024 ICHIRO ITS
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to deal
+// in the Software without restriction, including without limitation the rights
+// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+// copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+// THE SOFTWARE.
+
+#include <rclcpp/rclcpp.hpp>
+#include <shisen_cpp/config/grpc/call_data_record_image.hpp>
+#include <shisen_interfaces/shisen.grpc.pb.h>
+#include <shisen_interfaces/shisen.pb.h>
+
+namespace shisen_cpp
+{
+CallDataRecordImage::CallDataRecordImage(
+  shisen_interfaces::proto::Config::AsyncService * service, grpc::ServerCompletionQueue * cq,
+  const std::string & path, const std::shared_ptr<camera::CameraNode>& camera_node)
+: CallData(service, cq, path), camera_node_(camera_node)
+{
+  Proceed();
+}
+
+void CallDataRecordImage::AddNextToCompletionQueue()
+{
+  new CallDataRecordImage(service_, cq_, path_, camera_node_);
+}
+
+void CallDataRecordImage::WaitForRequest()
+{
+  service_->RequestRecordImage(&ctx_, &request_, &responder_, cq_, cq_, this);
+}
+
+void CallDataRecordImage::HandleRequest()
+{
+  try {
+    camera_node_->save_image(camera_node_->get_mat());
+  } catch(const std::exception& e) {
+    RCLCPP_ERROR(rclcpp::get_logger("Record Image"), e.what());
+  }
+}
+}  // namespace shisen_cpp

src/shisen_cpp/config/grpc/config.cpp

@@ -24,6 +24,7 @@
 #include <shisen_cpp/config/grpc/call_data_get_image.hpp>
 #include <shisen_cpp/config/grpc/call_data_save_capture_setting.hpp>
 #include <shisen_cpp/config/grpc/call_data_set_capture_setting.hpp>
+#include <shisen_cpp/config/grpc/call_data_record_image.hpp>
 #include <shisen_cpp/config/grpc/config.hpp>
 #include <jitsuyo/config.hpp>
 
@@ -72,6 +73,7 @@ void ConfigGrpc::Run(const std::string & path, std::shared_ptr<camera::CameraNod
     new CallDataSaveCaptureSetting(&service_, cq_.get(), path);
     new CallDataSetCaptureSetting(&service_, cq_.get(), path, camera_node);
     new CallDataGetImage(&service_, cq_.get(), path, camera_node);
+    new CallDataRecordImage(&service_, cq_.get(), path, camera_node);
     void * tag;  // uniquely identifies a request.
     bool ok = true;
     while (true) {