Merge branch 'master' of github.com:ichiro-its/suiryoku into feature/…

…speed-control-based-on-imu

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/locomotion/process/locomotion.hpp

@@ -59,6 +59,7 @@ class Locomotion
 
   bool dribble(const keisan::Angle<double> & direction, float delta_sec);
   bool pivot(const keisan::Angle<double> & direction, float delta_sec);
+  bool pivot_new(const keisan::Angle<double> & direction);
 
   bool position_until(
     const keisan::Angle<double> & target_pan,
@@ -71,7 +72,7 @@ class Locomotion
                                      const keisan::Angle<double> & max_pan, const keisan::Angle<double> & min_tilt, 
                                      const keisan::Angle<double> & max_tilt, float delta_sec);
   bool position_kick_general(const keisan::Angle<double> & direction, float delta_sec);
-  bool position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, float delta_sec);
+  bool position_kick_range_pan_tilt(const keisan::Angle<double> & direction, bool precise_kick, bool left_kick, bool is_positioning_center, float delta_sec);
 
   bool is_time_to_follow();
   bool pivot_fulfilled();
@@ -88,6 +89,9 @@ class Locomotion
 
   std::string config_name;
 
+  bool initial_pivot;
+  keisan::Angle<double> pivot_stop_limit;
+
 private:
 
   double move_min_x;
@@ -126,7 +130,7 @@ class Locomotion
 
   keisan::Angle<double> pivot_target_tilt;
   double pivot_max_delta_direction;
-  double pivot_pan_range_ratio;
+  double pivot_pan_range_a_speed;
   double pivot_min_x;
   double pivot_max_x;
   double pivot_max_ly;
@@ -150,7 +154,8 @@ class Locomotion
   keisan::Angle<double> position_min_range_tilt;
   keisan::Angle<double> position_max_range_tilt;
   keisan::Angle<double> position_min_range_pan;
-  keisan::Angle<double> position_center_range_pan;
+  keisan::Angle<double> position_center_right_range_pan;
+  keisan::Angle<double> position_center_left_range_pan;
   keisan::Angle<double> position_max_range_pan;
 
   double skew_max_x;

package.xml

@@ -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>

src/suiryoku/config/node/config_node.cpp

@@ -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));
     });
 }