Merge branch 'master' of github.com:ichiro-its/suiryoku into feature/adjust-function-for-dynamic-kick

Author: hiikariri

CMakeLists.txt

@@ -18,6 +18,7 @@ find_package(ament_index_cpp REQUIRED)
 find_package(aruku REQUIRED)
 find_package(aruku_interfaces REQUIRED)
 find_package(atama_interfaces REQUIRED)
+find_package(jitsuyo REQUIRED)
 find_package(kansei REQUIRED)
 find_package(kansei_interfaces REQUIRED)
 find_package(keisan REQUIRED)
@@ -31,7 +32,6 @@ include_directories(${EIGEN3_INCLUDE_DIRS})
 
 add_library(${PROJECT_NAME} SHARED
   "src/${PROJECT_NAME}/config/node/config_node.cpp"
-  "src/${PROJECT_NAME}/config/utils/config.cpp"
   "src/${PROJECT_NAME}/locomotion/control/helper/parameter.cpp"
   "src/${PROJECT_NAME}/locomotion/control/node/control_node.cpp"
   "src/${PROJECT_NAME}/locomotion/model/robot.cpp"
@@ -47,6 +47,7 @@ ament_target_dependencies(${PROJECT_NAME}
   aruku
   aruku_interfaces
   atama_interfaces
+  jitsuyo
   kansei
   kansei_interfaces
   keisan
@@ -111,6 +112,7 @@ ament_export_dependencies(
   aruku
   aruku_interfaces
   atama_interfaces
+  jitsuyo
   kansei
   kansei_interfaces
   keisan

include/suiryoku/config/config.hpp

@@ -21,7 +21,6 @@
 #ifndef SUIRYOKU__CONFIG__CONFIG_HPP_
 #define SUIRYOKU__CONFIG__CONFIG_HPP_
 
-#include "suiryoku/config/utils/config.hpp"
 #include "suiryoku/config/node/config_node.hpp"
 
 #endif  // SUIRYOKU__CONFIG__CONFIG_HPP_

include/suiryoku/config/node/config_node.hpp

@@ -25,7 +25,6 @@
 #include <string>
 
 #include "rclcpp/rclcpp.hpp"
-#include "suiryoku/config/utils/config.hpp"
 #include "suiryoku_interfaces/msg/set_config.hpp"
 #include "suiryoku_interfaces/srv/get_config.hpp"
 #include "suiryoku_interfaces/srv/save_config.hpp"
@@ -48,7 +47,6 @@ class ConfigNode
 private:
   std::string get_node_prefix() const;
 
-  Config config;
   rclcpp::Node::SharedPtr node;
 
   rclcpp::Service<GetConfig>::SharedPtr get_config_server;

include/suiryoku/config/utils/config.hpp

@@ -11,6 +11,7 @@
   <depend>aruku</depend>
   <depend>aruku_interfaces</depend>
   <depend>atama_interfaces</depend>
+  <depend>jitsuyo</depend>
   <depend>kansei</depend>
   <depend>kansei_interfaces</depend>
   <depend>keisan</depend>

package.xml

@@ -23,38 +23,31 @@
 #include <string>
 
 #include "suiryoku/config/node/config_node.hpp"
+#include "jitsuyo/config.hpp"
 
 #include "nlohmann/json.hpp"
 #include "rclcpp/rclcpp.hpp"
-#include "suiryoku/config/utils/config.hpp"
 
 namespace suiryoku
 {
 
 ConfigNode::ConfigNode(rclcpp::Node::SharedPtr node, const std::string & path)
-: node(node), config(path), set_config_subscriber(nullptr)
+: node(node), set_config_subscriber(nullptr)
 {
   get_config_server = node->create_service<GetConfig>(
     get_node_prefix() + "/get_config",
-    [this](GetConfig::Request::SharedPtr request, GetConfig::Response::SharedPtr response) {
-      response->json = this->config.get_config();
+    [this, path](GetConfig::Request::SharedPtr request, GetConfig::Response::SharedPtr response) {
+      nlohmann::json data;
+      if (!jitsuyo::load_config(path, "/head.json", data)) {
+        return;
+      }
+      response->json = data.dump();
     });
 
   save_config_server = node->create_service<SaveConfig>(
     get_node_prefix() + "/save_config",
-    [this](SaveConfig::Request::SharedPtr request, SaveConfig::Response::SharedPtr response) {
-      try {
-        nlohmann::json data = nlohmann::json::parse(request->json);
-
-        this->config.save_config(data);
-        response->status = true;
-      } catch (std::ofstream::failure) {
-        // TODO(maroqijalil): log it
-        response->status = false;
-      } catch (nlohmann::json::exception) {
-        // TODO(maroqijalil): log it
-        response->status = false;
-      }
+    [this, path](SaveConfig::Request::SharedPtr request, SaveConfig::Response::SharedPtr response) {
+      response->status = jitsuyo::save_config(path, "locomotion.json", nlohmann::json::parse(request->json));
     });
 }