diff --git a/CMakeLists.txt b/CMakeLists.txt
index babce85..3f3a7c4 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/package.xml b/package.xml
index 41fda22..2ad8100 100644
--- a/package.xml
+++ b/package.xml
@@ -8,6 +8,7 @@
MIT License
ament_cmake
dynamixel_sdk
+ jitsuyo
kansei_interfaces
keisan
rclcpp
diff --git a/src/tachimawari/joint/tf2/tf2_manager.cpp b/src/tachimawari/joint/tf2/tf2_manager.cpp
index 6888f25..df238de 100644
--- a/src/tachimawari/joint/tf2/tf2_manager.cpp
+++ b/src/tachimawari/joint/tf2/tf2_manager.cpp
@@ -21,12 +21,14 @@
#include
#include
#include
-#include
#include
#include
#include "tachimawari/joint/tf2/tf2_manager.hpp"
+#include "jitsuyo/config.hpp"
+#include "nlohmann/json.hpp"
+
namespace tachimawari::joint
{
@@ -34,34 +36,73 @@ 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;