From 6b579afa26310d92639289baf1d20e094fafcc35 Mon Sep 17 00:00:00 2001
From: mbsaloka <mbsaloka@gmail.com>
Date: Wed, 10 Jul 2024 23:55:38 +0700
Subject: [PATCH] feat: set width and height from config

---
 .../shisen_cpp/camera/provider/image_provider.hpp   |  1 +
 src/shisen_cpp/camera/node/camera_node.cpp          | 11 ++++++++++-
 src/shisen_cpp/camera/provider/image_provider.cpp   | 13 +++++++++----
 .../config/grpc/call_data_get_capture_setting.cpp   |  2 ++
 .../config/grpc/call_data_save_capture_setting.cpp  | 11 ++++++++++-
 5 files changed, 32 insertions(+), 6 deletions(-)

diff --git a/include/shisen_cpp/camera/provider/image_provider.hpp b/include/shisen_cpp/camera/provider/image_provider.hpp
index 9104bb8..a2c0a3d 100644
--- a/include/shisen_cpp/camera/provider/image_provider.hpp
+++ b/include/shisen_cpp/camera/provider/image_provider.hpp
@@ -45,6 +45,7 @@ class ImageProvider
   void set_image(const Image & image);
   void set_mat(cv::Mat mat);
   void update_mat();
+  void initialize_video_capture(const Options & options);
 
   const Image & get_image() const;
   cv::Mat get_mat() const;
diff --git a/src/shisen_cpp/camera/node/camera_node.cpp b/src/shisen_cpp/camera/node/camera_node.cpp
index 99631f5..f28b907 100644
--- a/src/shisen_cpp/camera/node/camera_node.cpp
+++ b/src/shisen_cpp/camera/node/camera_node.cpp
@@ -237,13 +237,17 @@ void CameraNode::load_configuration(const std::string & path)
   int setting_temperature;
   int setting_exposure;
   int setting_gain;
+  int setting_width;
+  int setting_height;
 
   if (!jitsuyo::assign_val(config, "brightness", setting_brightness) ||
     !jitsuyo::assign_val(config, "contrast", setting_contrast) ||
     !jitsuyo::assign_val(config, "saturation", setting_saturation) ||
     !jitsuyo::assign_val(config, "temperature", setting_temperature) ||
     !jitsuyo::assign_val(config, "exposure", setting_exposure) ||
-    !jitsuyo::assign_val(config, "gain", setting_gain))
+    !jitsuyo::assign_val(config, "gain", setting_gain) ||
+    !jitsuyo::assign_val(config, "width", setting_width) ||
+    !jitsuyo::assign_val(config, "height", setting_height))
   {
     std::cout << "Error found at section `capture_settings`" << std::endl;
     throw std::runtime_error("Failed to load config file `" + path + "capture_settings.json`");
@@ -257,6 +261,11 @@ void CameraNode::load_configuration(const std::string & path)
   capture_setting.gain.set(setting_gain);
 
   configure_capture_setting(capture_setting);
+
+  options.width = setting_width;
+  options.height = setting_height;
+
+  image_provider->initialize_video_capture(options);
 }
 
 }  // namespace shisen_cpp::camera
diff --git a/src/shisen_cpp/camera/provider/image_provider.cpp b/src/shisen_cpp/camera/provider/image_provider.cpp
index e870733..b91ae73 100644
--- a/src/shisen_cpp/camera/provider/image_provider.cpp
+++ b/src/shisen_cpp/camera/provider/image_provider.cpp
@@ -36,6 +36,15 @@ ImageProvider::ImageProvider(const Options & options)
     throw std::runtime_error("unable to open camera on `" + options.camera_file_name + "`");
   }
 
+  initialize_video_capture(options);
+}
+
+ImageProvider::~ImageProvider()
+{
+}
+
+void ImageProvider::initialize_video_capture(const Options & options)
+{
   video_capture->set(cv::CAP_PROP_FRAME_WIDTH, options.width);
   video_capture->set(cv::CAP_PROP_FRAME_HEIGHT, options.height);
   video_capture->set(cv::CAP_PROP_AUTOFOCUS, 0);   // Disable autofocus
@@ -45,10 +54,6 @@ ImageProvider::ImageProvider(const Options & options)
   video_capture->set(cv::CAP_PROP_FOCUS, 0); // Set focus to 0
 }
 
-ImageProvider::~ImageProvider()
-{
-}
-
 void ImageProvider::set_image(const Image & image)
 {
   current_image_msg = image;
diff --git a/src/shisen_cpp/config/grpc/call_data_get_capture_setting.cpp b/src/shisen_cpp/config/grpc/call_data_get_capture_setting.cpp
index bfedc23..1cccc38 100644
--- a/src/shisen_cpp/config/grpc/call_data_get_capture_setting.cpp
+++ b/src/shisen_cpp/config/grpc/call_data_get_capture_setting.cpp
@@ -51,6 +51,8 @@ void CallDataGetCaptureSetting::HandleRequest()
     RCLCPP_ERROR(rclcpp::get_logger("Get config"), "Failed to load config!");
     return;
   }
+  data.erase("width");
+  data.erase("height");
   std::string capture_setting = data.dump();
   try {
     reply_.set_json_capture(capture_setting);
diff --git a/src/shisen_cpp/config/grpc/call_data_save_capture_setting.cpp b/src/shisen_cpp/config/grpc/call_data_save_capture_setting.cpp
index 8a0d0f7..4623539 100644
--- a/src/shisen_cpp/config/grpc/call_data_save_capture_setting.cpp
+++ b/src/shisen_cpp/config/grpc/call_data_save_capture_setting.cpp
@@ -52,8 +52,17 @@ void CallDataSaveCaptureSetting::HandleRequest()
     std::replace(json_string.begin(), json_string.end(), '\\', ' ');
     nlohmann::json capture_data = nlohmann::json::parse(json_string);
 
+    nlohmann::json data;
+    if (!jitsuyo::load_config(path_, "capture_settings.json", data)) {
+      RCLCPP_ERROR(rclcpp::get_logger("Get config"), "Failed to load config!");
+      return;
+    }
+
+    capture_data["width"] = data["width"];
+    capture_data["height"] = data["height"];
+
     jitsuyo::save_config(path_, "capture_settings.json", capture_data);
-    RCLCPP_INFO(rclcpp::get_logger("Save config"), "config has been saved!  ");
+    RCLCPP_INFO(rclcpp::get_logger("Save config"), "config has been saved!");
   } catch (nlohmann::json::exception & e) {
     RCLCPP_ERROR(rclcpp::get_logger("Save config"), e.what());
   }