Merge pull request #39 from ichiro-its/enhancement/use-jitsuyo-utils

[Sprint 22/23 | PD-421] - [Enhancement] Use Jitsuyo For Utilities

CMakeLists.txt

@@ -15,6 +15,7 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(jitsuyo REQUIRED)
 find_package(kansei_interfaces REQUIRED)
 find_package(keisan REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -40,6 +41,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
 
 ament_target_dependencies(${PROJECT_NAME}
   geometry_msgs
+  jitsuyo
   keisan
   kansei_interfaces
   rclcpp
@@ -94,6 +96,7 @@ endif()
 
 ament_export_dependencies(
   geometry_msgs
+  jitsuyo
   keisan
   kansei_interfaces
   rclcpp

package.xml

@@ -8,6 +8,7 @@
   <license>MIT License</license>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>geometry_msgs</depend>
+  <depend>jitsuyo</depend>
   <depend>keisan</depend>
   <depend>kansei_interfaces</depend>
   <depend>nlohmann-json-dev</depend>

src/kansei/fallen/node/fallen_determinant.cpp

@@ -24,6 +24,7 @@
 
 #include "kansei/fallen/fallen.hpp"
 
+#include "jitsuyo/config.hpp"
 #include "keisan/keisan.hpp"
 #include "nlohmann/json.hpp"
 
@@ -39,37 +40,45 @@ FallenDeterminant::FallenDeterminant(const DeterminantType & type)
 
 void FallenDeterminant::load_config(const std::string & path)
 {
-  std::string file_name = path + "kansei.json";
-  std::ifstream file(file_name);
+  nlohmann::json imu_data;
+  if (!jitsuyo::load_config(path, "kansei.json", imu_data)) {
+    throw std::runtime_error("Failed to find config file `" + path + "kansei.json`");
+  }
 
-  if (!file.is_open()) {
-    throw std::runtime_error("Failed to open file: " + file_name);
+  bool valid_config = true;
+
+  nlohmann::json accel_limit_section;
+  if (jitsuyo::assign_val(imu_data, "accel_limit", accel_limit_section)) {
+    bool valid_section = true;
+    valid_section &= jitsuyo::assign_val(accel_limit_section, "accel_back_limit", accel_back_limit);
+    valid_section &= jitsuyo::assign_val(accel_limit_section, "accel_front_limit", accel_front_limit);
+    valid_section &= jitsuyo::assign_val(accel_limit_section, "accel_right_limit", accel_right_limit);
+    valid_section &= jitsuyo::assign_val(accel_limit_section, "accel_left_limit", accel_left_limit);
+    if (!valid_section) {
+      std::cout << "Error found at section `accel_limit`" << std::endl;
+      valid_config = false;
+    }
+  } else {
+    valid_config = false;
   }
 
-  nlohmann::json imu_data = nlohmann::json::parse(file);
-
-  for (const auto &[key, val] : imu_data.items()) {
-    if (key == "accel_limit") {
-      try {
-        val.at("accel_back_limit").get_to(accel_back_limit);
-        val.at("accel_front_limit").get_to(accel_front_limit);
-        val.at("accel_right_limit").get_to(accel_right_limit);
-        val.at("accel_left_limit").get_to(accel_left_limit);
-      } catch (nlohmann::json::parse_error & ex) {
-        std::cerr << "parse error at byte " << ex.byte << std::endl;
-        throw ex;
-      }
-    } else if (key == "orientation_limit") {
-      try {
-        val.at("pitch_back_limit").get_to(pitch_back_limit);
-        val.at("pitch_front_limit").get_to(pitch_front_limit);
-        val.at("roll_right_limit").get_to(roll_right_limit);
-        val.at("roll_left_limit").get_to(roll_left_limit);
-      } catch (nlohmann::json::parse_error & ex) {
-        std::cerr << "parse error at byte " << ex.byte << std::endl;
-        throw ex;
-      }
+  nlohmann::json orientation_limit_section;
+  if (jitsuyo::assign_val(imu_data, "orientation_limit", orientation_limit_section)) {
+    bool valid_section = true;
+    valid_section &= jitsuyo::assign_val(orientation_limit_section, "pitch_back_limit", pitch_back_limit);
+    valid_section &= jitsuyo::assign_val(orientation_limit_section, "pitch_front_limit", pitch_front_limit);
+    valid_section &= jitsuyo::assign_val(orientation_limit_section, "roll_right_limit", roll_right_limit);
+    valid_section &= jitsuyo::assign_val(orientation_limit_section, "roll_left_limit", roll_left_limit);
+    if (!valid_section) {
+      std::cout << "Error found at section `orientation_limit`" << std::endl;
+      valid_config = false;
     }
+  } else {
+    valid_config = false;
+  }
+
+  if (!valid_config) {
+    throw std::runtime_error("Failed to load config file `" + path + "kansei.json`");
   }
 }
 

src/kansei/measurement/filter/filter.cpp

@@ -29,6 +29,7 @@
 #include "kansei/measurement/filter/filter.hpp"
 
 #include "geometry_msgs/msg/vector3_stamped.hpp"
+#include "jitsuyo/config.hpp"
 #include "kansei/measurement/filter/madgwick/madgwick.hpp"
 #include "keisan/keisan.hpp"
 #include "nlohmann/json.hpp"
@@ -50,21 +51,29 @@ Filter::Filter()
 
 void Filter::load_config(const std::string & path)
 {
-  std::string file_name =
-    path + "imu/" + "kansei.json";
-  std::ifstream file(file_name);
-  nlohmann::json imu_data = nlohmann::json::parse(file);
-
-  for (const auto &[key, val] : imu_data.items()) {
-    if (key == "filter") {
-      try {
-        val.at("gy_mux_x").get_to(raw_gy_mux[0]);
-        val.at("gy_mux_y").get_to(raw_gy_mux[1]);
-        val.at("gy_mux_z").get_to(raw_gy_mux[2]);
-      } catch (nlohmann::json::parse_error & ex) {
-        std::cerr << "parse error at byte " << ex.byte << std::endl;
-      }
+  nlohmann::json imu_data;
+  if (!jitsuyo::load_config(path, "imu/kansei.json", imu_data)) {
+    throw std::runtime_error("Failed to find config file `" + path + "imu/kansei.json`");
+  }
+
+  bool valid_config = true;
+
+  nlohmann::json filter_section;
+  if (jitsuyo::assign_val(imu_data, "filter", filter_section)) {
+    bool valid_section = true;
+    valid_section &= jitsuyo::assign_val(filter_section, "gyro_mux_x", raw_gy_mux[0]);
+    valid_section &= jitsuyo::assign_val(filter_section, "gyro_mux_y", raw_gy_mux[1]);
+    valid_section &= jitsuyo::assign_val(filter_section, "gyro_mux_z", raw_gy_mux[2]);
+    if (!valid_section) {
+      std::cout << "Error found at section `filter`" << std::endl;
+      valid_config = false;
     }
+  } else {
+    valid_config = false;
+  }
+
+  if (!valid_config) {
+    throw std::runtime_error("Failed to load config file `" + path + "imu/kansei.json`");
   }
 }