Merge pull request #64 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(dynamixel_sdk REQUIRED)
+find_package(jitsuyo REQUIRED)
 find_package(kansei_interfaces REQUIRED)
 find_package(keisan REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -70,6 +71,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
 
 ament_target_dependencies(${PROJECT_NAME}
   dynamixel_sdk
+  jitsuyo
   kansei_interfaces
   keisan
   rclcpp
@@ -170,6 +172,7 @@ endif()
 
 ament_export_dependencies(
   dynamixel_sdk
+  jitsuyo
   kansei_interfaces
   keisan
   rclcpp

package.xml

@@ -8,6 +8,7 @@
   <license>MIT License</license>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>dynamixel_sdk</depend>
+  <depend>jitsuyo</depend>
   <depend>kansei_interfaces</depend>
   <depend>keisan</depend>
   <depend>rclcpp</depend>

src/tachimawari/joint/tf2/tf2_manager.cpp

@@ -21,47 +21,88 @@
 #include <algorithm>
 #include <fstream>
 #include <memory>
-#include <nlohmann/json.hpp>
 #include <string>
 #include <vector>
 
 #include "tachimawari/joint/tf2/tf2_manager.hpp"
 
+#include "jitsuyo/config.hpp"
+#include "nlohmann/json.hpp"
+
 namespace tachimawari::joint
 {
 
 Tf2Manager::Tf2Manager() {}
 
 bool Tf2Manager::load_configuration(const std::string & path)
 {
-  std::string ss = path + "frame_measurements.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, "frame_measurements.json", config)) {
+    return false;
   }
 
-  nlohmann::json config = nlohmann::json::parse(input);
+  bool valid_config = true;
 
   for (auto & item : config.items()) {
     // Get all config
-    try {
-      std::string name = item.key();
-      double translation_x = item.value().at("translation").at("x");
-      double translation_y = item.value().at("translation").at("y");
-      double translation_z = item.value().at("translation").at("z");
-      double const_roll = item.value().at("const_rpy").at("roll");
-      double const_pitch = item.value().at("const_rpy").at("pitch");
-      double const_yaw = item.value().at("const_rpy").at("yaw");
-      auto map_string_id = FrameId::frame_string_id.find(name);
-
-      uint8_t id = map_string_id->second;
-      frames.push_back(
-        Frame(id, translation_x, translation_y, translation_z, const_roll, const_pitch, const_yaw));
-    } catch (nlohmann::json::parse_error & ex) {
-      std::cerr << "parse error at byte " << ex.byte << std::endl;
-      throw ex;
+    double translation_x;
+    double translation_y;
+    double translation_z;
+    double const_roll;
+    double const_pitch;
+    double const_yaw;
+
+    std::string name = item.key();
+
+    bool valid_section = true;
+    nlohmann::json section;
+    if (jitsuyo::assign_val(config, name, section)) {
+      nlohmann::json translation;
+      nlohmann::json const_rpy;
+
+      if (jitsuyo::assign_val(section, "translation", translation)) {
+        bool valid_translation = true;
+        valid_translation &= jitsuyo::assign_val(translation, "x", translation_x);
+        valid_translation &= jitsuyo::assign_val(translation, "y", translation_y);
+        valid_translation &= jitsuyo::assign_val(translation, "z", translation_z);
+        if (!valid_translation) {
+          std::cout << "Error found at section `translation`" << std::endl;
+          valid_section = false;
+        }
+      } else {
+        valid_section = false;
+      }
+
+      if (jitsuyo::assign_val(section, "const_rpy", const_rpy)) {
+        bool valid_const_rpy = true;
+        valid_const_rpy &= jitsuyo::assign_val(const_rpy, "roll", const_roll);
+        valid_const_rpy &= jitsuyo::assign_val(const_rpy, "pitch", const_pitch);
+        valid_const_rpy &= jitsuyo::assign_val(const_rpy, "yaw", const_yaw);
+        if (!valid_const_rpy) {
+          std::cout << "Error found at section `const_rpy`" << std::endl;
+          valid_section = false;
+        }
+      } else {
+        valid_section = false;
+      }
+    } else {
+      valid_section = false;
     }
+
+    if (!valid_section) {
+      valid_config = false;
+      continue;
+    }
+
+    auto map_string_id = FrameId::frame_string_id.find(name);
+
+    uint8_t id = map_string_id->second;
+    frames.push_back(
+      Frame(id, translation_x, translation_y, translation_z, const_roll, const_pitch, const_yaw));
+  }
+
+  if (!valid_config) {
+    throw std::runtime_error("Failed to load config file `" + path + "frame_measurements.json`");
   }
 
   return true;