Merge branch 'master' into enhancement/load-config-for-server

Author: mbsaloka

CMakeLists.txt

@@ -20,6 +20,7 @@ find_package(keisan REQUIRED)
 find_package(OpenCV REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(shisen_interfaces REQUIRED)
+find_package(jitsuyo REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(Protobuf CONFIG REQUIRED)
@@ -40,7 +41,7 @@ add_library(${PROJECT_NAME} SHARED
   "src/${PROJECT_NAME}/config/grpc/call_data_load_config.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/utils/config.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"
@@ -73,6 +74,7 @@ ament_target_dependencies(${PROJECT_NAME}
   OpenCV
   rclcpp
   shisen_interfaces
+  jitsuyo
   sensor_msgs
   cv_bridge
   std_msgs
@@ -83,6 +85,7 @@ ament_target_dependencies(${PROJECT_NAME}_exported
   OpenCV
   rclcpp
   shisen_interfaces
+  jitsuyo
   sensor_msgs
   cv_bridge
   std_msgs)
@@ -94,6 +97,10 @@ target_link_libraries(${PROJECT_NAME}
 
 install(DIRECTORY "include" DESTINATION ".")
 
+install(DIRECTORY
+  launch
+  DESTINATION "share/${PROJECT_NAME}")
+
 install(TARGETS ${PROJECT_NAME}
 EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION "lib"

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);

…clude/shisen_cpp/config/utils/config.hpp …p/config/grpc/call_data_record_image.hppinclude/shisen_cpp/config/utils/config.hpp renamed to include/shisen_cpp/config/grpc/call_data_record_image.hpp include/shisen_cpp/config/utils/config.hpp renamed to include/shisen_cpp/config/grpc/call_data_record_image.hpp

@@ -18,27 +18,28 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 // THE SOFTWARE.
 
-#ifndef SHISEN_CPP__CONFIG__UTILS__CONFIG_HPP_
-#define SHISEN_CPP__CONFIG__UTILS__CONFIG_HPP_
+#ifndef SHISEN_CPP__CONFIG__GRPC__CALL_DATA_RECORD_IMAGE_HPP__
+#define SHISEN_CPP__CONFIG__GRPC__CALL_DATA_RECORD_IMAGE_HPP__
 
-#include <nlohmann/json.hpp>
+#include <shisen_cpp/camera/node/camera_node.hpp>
+#include <shisen_cpp/config/grpc/call_data.hpp>
 
 namespace shisen_cpp
 {
-
-class Config
+class CallDataRecordImage
+: CallData<shisen_interfaces::proto::Empty, shisen_interfaces::proto::Empty>
 {
 public:
-  explicit Config(const std::string & path);
-
-  std::string get_capture_setting(const std::string & key) const;
-  void save_capture_setting(const nlohmann::json & capture_data);
-  nlohmann::json get_grpc_config() const;
-
-private:
-  std::string path;
+  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__UTILS__CONFIG_HPP_
+#endif  // SHISEN_CPP__CONFIG__GRPC__CALL_DATA_RECORD_IMAGE_HPP__

include/shisen_cpp/node/shisen_cpp_node.hpp

@@ -24,7 +24,6 @@
 #include <rclcpp/rclcpp.hpp>
 #include <shisen_cpp/camera/node/camera_node.hpp>
 #include <shisen_cpp/config/grpc/config.hpp>
-#include <shisen_cpp/config/utils/config.hpp>
 #include <shisen_cpp/utility.hpp>
 
 #include <memory>

launch/shisen_cpp_launch.py

@@ -0,0 +1,40 @@
+# 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.
+
+import os
+import socket
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    hostname = socket.gethostname()
+    config_path = os.path.expanduser(f'~/ros2-ws/configuration/{hostname}/camera/')
+
+    return LaunchDescription([
+        Node(
+            package='shisen_cpp',
+            executable='camera',
+            name='camera',
+            output='screen',
+            arguments=[config_path],
+            respawn=True,
+            respawn_delay=1
+        )
+    ])

package.xml

@@ -8,6 +8,7 @@
   <license>MIT License</license>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>cv_bridge</depend>
+  <depend>jitsuyo</depend>
   <depend>keisan</depend>
   <depend>libopencv-dev</depend>
   <depend>rclcpp</depend>

src/shisen_cpp/camera/node/camera_node.cpp

@@ -18,11 +18,16 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 // THE SOFTWARE.
 
+
 #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
@@ -64,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();
@@ -161,12 +192,10 @@ CaptureSetting CameraNode::on_configure_capture_setting(
     }
 
     if (new_capture_setting.temperature.is_not_empty()) {
-      video_capture->set(cv::CAP_PROP_AUTO_WB, 0);
       video_capture->set(cv::CAP_PROP_WB_TEMPERATURE, new_capture_setting.temperature);
     }
 
     if (new_capture_setting.exposure.is_not_empty()) {
-      video_capture->set(cv::CAP_PROP_AUTO_EXPOSURE, 1);
       video_capture->set(cv::CAP_PROP_EXPOSURE, new_capture_setting.exposure);
     }
 
@@ -195,38 +224,38 @@ void CameraNode::configure_capture_setting(const CaptureSetting & capture_settin
 
 void CameraNode::load_configuration(const std::string & path)
 {
-  std::string ss = path + "capture_settings.json";
-
-  std::ifstream input(ss, std::ifstream::in);
-  if (!input.is_open()) {
-    throw std::runtime_error("Unable to open `" + ss + "`!");
+  nlohmann::json config;
+  if (!jitsuyo::load_config(path, "capture_settings.json", config)) {
+    throw std::runtime_error("Unable to open `" + path + "capture_settings.json`!");
   }
 
-  nlohmann::json config = nlohmann::json::parse(input);
-
   CaptureSetting capture_setting;
 
-  // Get all config
-  for (auto & item : config.items()) {
-    try {
-      if (item.key() == "brightness") {
-        capture_setting.brightness.set(item.value());
-      } else if (item.key() == "contrast") {
-        capture_setting.contrast.set(item.value());
-      } else if (item.key() == "saturation") {
-        capture_setting.saturation.set(item.value());
-      } else if (item.key() == "temperature") {
-        capture_setting.temperature.set(item.value());
-      } else if (item.key() == "exposure") {
-        capture_setting.exposure.set(item.value());
-      } else if (item.key() == "gain") {
-        capture_setting.gain.set(item.value());
-      }
-    } catch (nlohmann::json::parse_error & ex) {
-      throw std::runtime_error("Parse error at byte `" + std::to_string(ex.byte) + "`!");
-    }
+  int setting_brightness;
+  int setting_contrast;
+  int setting_saturation;
+  int setting_temperature;
+  int setting_exposure;
+  int setting_gain;
+
+  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))
+  {
+    std::cout << "Error found at section `capture_settings`" << std::endl;
+    throw std::runtime_error("Failed to load config file `" + path + "capture_settings.json`");
   }
 
+  capture_setting.brightness.set(setting_brightness);
+  capture_setting.contrast.set(setting_contrast);
+  capture_setting.saturation.set(setting_saturation);
+  capture_setting.temperature.set(setting_temperature);
+  capture_setting.exposure.set(setting_exposure);
+  capture_setting.gain.set(setting_gain);
+
   configure_capture_setting(capture_setting);
 }
 

src/shisen_cpp/camera/provider/image_provider.cpp

@@ -38,6 +38,11 @@ ImageProvider::ImageProvider(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
+  video_capture->set(cv::CAP_PROP_AUTO_WB, 0);     // Disable auto white balance
+  video_capture->set(cv::CAP_PROP_AUTO_EXPOSURE, 1);  // Set auto exposure to manual mode
+  video_capture->set(cv::CAP_PROP_SHARPNESS, 0); // Set sharpness to 0
+  video_capture->set(cv::CAP_PROP_FOCUS, 0); // Set focus to 0
 }
 
 ImageProvider::~ImageProvider()

src/shisen_cpp/config/grpc/call_data_get_capture_setting.cpp

@@ -20,9 +20,9 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <shisen_cpp/config/grpc/call_data_get_capture_setting.hpp>
-#include <shisen_cpp/config/utils/config.hpp>
 #include <shisen_interfaces/shisen.grpc.pb.h>
 #include <shisen_interfaces/shisen.pb.h>
+#include <jitsuyo/config.hpp>
 
 namespace shisen_cpp
 {
@@ -46,9 +46,14 @@ void CallDataGetCaptureSetting::WaitForRequest()
 
 void CallDataGetCaptureSetting::HandleRequest()
 {
-  Config config(path_);
+  nlohmann::json data;
+  if (!jitsuyo::load_config(path_, "capture_settings.json", data)) {
+    RCLCPP_ERROR(rclcpp::get_logger("Get config"), "Failed to load config!");
+    return;
+  }
+  std::string capture_setting = data.dump();
   try {
-    reply_.set_json_capture(config.get_capture_setting("capture"));
+    reply_.set_json_capture(capture_setting);
     RCLCPP_INFO(rclcpp::get_logger("Get config"), "config has been sent!");
   } catch (nlohmann::json::exception & e) {
     RCLCPP_ERROR(rclcpp::get_logger("Publish config"), e.what());

src/shisen_cpp/config/utils/config.cpp …p/config/grpc/call_data_record_image.cppsrc/shisen_cpp/config/utils/config.cpp renamed to src/shisen_cpp/config/grpc/call_data_record_image.cpp src/shisen_cpp/config/utils/config.cpp renamed to src/shisen_cpp/config/grpc/call_data_record_image.cpp

@@ -18,40 +18,37 @@
 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 // THE SOFTWARE.
 
-#include <shisen_cpp/config/utils/config.hpp>
-
-#include <fstream>
-#include <iomanip>
-#include <string>
+#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
 {
-Config::Config(const std::string & path) : path(path) {}
-
-std::string Config::get_capture_setting(const std::string & key) const
+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)
 {
-  if (key == "capture") {
-    std::ifstream capture_file(path + "capture_settings.json");
-    nlohmann::json capture_data = nlohmann::json::parse(capture_file);
-    return capture_data.dump();
-  }
-
-  return "";
+  Proceed();
 }
 
-nlohmann::json Config::get_grpc_config() const
+void CallDataRecordImage::AddNextToCompletionQueue()
 {
-  std::ifstream grpc_file(path + "grpc.json");
-  nlohmann::json grpc_data = nlohmann::json::parse(grpc_file);
-  grpc_file.close();
-  return grpc_data;
+  new CallDataRecordImage(service_, cq_, path_, camera_node_);
 }
 
-void Config::save_capture_setting(const nlohmann::json & capture_data)
+void CallDataRecordImage::WaitForRequest()
 {
-  std::ofstream capture_file(path + "capture_settings.json", std::ios::out | std::ios::trunc);
-  capture_file << std::setw(2) << capture_data << std::endl;
-  capture_file.close();
+  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