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;