[WIP][Feature] Implement map action for dynamic kick

include/akushon/action/model/action.hpp

@@ -57,6 +57,8 @@ class Action
 
   void reset();
 
+  void map_action(const Action & source_action, const Action & target_action, int target_pose_index, float source_val, float source_min, float source_max);
+
   bool time_based;
 private:
   std::string name;

include/akushon/action/node/action_manager.hpp

@@ -45,10 +45,12 @@ class ActionManager
   Action get_action(std::string action_name) const;
 
   void load_config(const std::string & path);
+  void set_config(const std::string & path);
 
   Action load_action(const nlohmann::json & action_data, const std::string & action_name) const;
 
   void start(std::string action_name, const Pose & initial_pose);
+  void start(std::string action_name, std::string target_action_name, const Pose & initial_pose, float ball_x, float right_map_x_min_, float right_map_x_max_, float left_map_x_min_, float left_map_x_max_, bool right);
   void start(const Action & action, const Pose & initial_pose);
   void brake();
   void process(double time);
@@ -57,11 +59,19 @@ class ActionManager
 
   std::vector<tachimawari::joint::Joint> get_joints() const;
 
+  bool using_dynamic_kick;
+  double right_map_x_min;
+  double right_map_x_max;
+  double left_map_x_min;
+  double left_map_x_max;
+
 private:
   std::map<std::string, Action> actions;
 
   std::shared_ptr<Interpolator> interpolator;
   bool is_running;
+  std::string config_name;
+  std::string action_dir;
 };
 
 }  // namespace akushon

include/akushon/action/node/action_node.hpp

@@ -31,8 +31,10 @@
 #include "akushon_interfaces/msg/status.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/empty.hpp"
+#include "std_msgs/msg/float64.hpp"
 #include "tachimawari_interfaces/msg/current_joints.hpp"
 #include "tachimawari_interfaces/msg/set_joints.hpp"
+#include "keisan/keisan.hpp"
 
 namespace akushon
 {
@@ -45,6 +47,7 @@ class ActionNode
   using RunAction = akushon_interfaces::msg::RunAction;
   using SetJoints = tachimawari_interfaces::msg::SetJoints;
   using Status = akushon_interfaces::msg::Status;
+  using Float64 = std_msgs::msg::Float64;
 
   enum { READY, PLAYING };
 
@@ -54,6 +57,7 @@ class ActionNode
   static std::string run_action_topic();
   static std::string brake_action_topic();
   static std::string status_topic();
+  static std::string ball_topic();
 
   explicit ActionNode(
     rclcpp::Node::SharedPtr node, std::shared_ptr<ActionManager> & action_manager);
@@ -68,6 +72,8 @@ class ActionNode
   void publish_status();
 
   Pose initial_pose;
+  double ball_pos;
+
   rclcpp::Node::SharedPtr node;
 
   std::shared_ptr<ActionManager> action_manager;
@@ -77,6 +83,7 @@ class ActionNode
 
   rclcpp::Subscription<RunAction>::SharedPtr run_action_subscriber;
   rclcpp::Subscription<Empty>::SharedPtr brake_action_subscriber;
+  rclcpp::Subscription<Float64>::SharedPtr ball_subscriber;
   rclcpp::Publisher<Status>::SharedPtr status_publisher;
 };
 

include/akushon/config/utils/config.hpp

@@ -34,9 +34,11 @@ class Config
 
   std::string get_config() const;
   void save_config(const std::string & actions_data);
+  void clear_directory(const std::string& directory_path);
 
 private:
   std::string path;
+  std::string action_dir;
 };
 
 }  // namespace akushon

src/akushon/action/model/action.cpp

@@ -24,7 +24,7 @@
 #include <vector>
 
 #include "akushon/action/model/action.hpp"
-
+#include "rclcpp/rclcpp.hpp"
 #include "akushon/action/model/pose.hpp"
 
 namespace akushon
@@ -110,4 +110,48 @@ void Action::reset()
   poses.clear();
 }
 
+void Action::map_action(const Action & source_action, const Action & target_action, int target_pose_index, float source_val, float source_min, float source_max)
+{
+  std::vector<tachimawari::joint::Joint> src_joints = source_action.get_pose(target_pose_index).get_joints();
+  std::vector<tachimawari::joint::Joint> target_joints = target_action.get_pose(target_pose_index).get_joints();
+  for (int joint = src_joints.size() - 1; joint >= 0; --joint)
+  {
+      int joint_id = src_joints.at(joint).get_id();
+      float target_min = src_joints.at(joint).get_position_value();
+      float target_max = target_joints.at(joint).get_position_value();
+      float new_joint_value = keisan::map(source_val, source_min, source_max, target_min, target_max);
+      new_joint_value = keisan::curve(new_joint_value, target_min, target_max, float(2.0));
+      new_joint_value = keisan::clamp(new_joint_value, target_min, target_max);
+      target_joints[joint].set_position_value(static_cast<int>(std::round(new_joint_value)));
+      RCLCPP_INFO(rclcpp::get_logger("Map Action"), "Joint %d: %f -> %d", joint_id, target_min, int(std::round(new_joint_value)));
+      RCLCPP_INFO(rclcpp::get_logger("Map Action"), "Cek value in target joints %d: %f -> %d", target_joints[joint].get_id(), target_max, target_joints[joint].get_position_value());
+  }
+  Pose new_pose(target_action.get_pose(target_pose_index).get_name());
+  new_pose.set_pause(source_action.get_pose(target_pose_index).get_pause());
+  new_pose.set_speed(source_action.get_pose(target_pose_index).get_speed());
+  new_pose.action_time = source_action.get_pose(target_pose_index).action_time;
+  new_pose.set_joints(target_joints);
+
+  RCLCPP_INFO(rclcpp::get_logger("Map Action"), "Cek new pose name %s", new_pose.get_name().c_str());
+  RCLCPP_INFO(rclcpp::get_logger("Map Action"), "Cek new pose pause %f", new_pose.get_pause());
+  RCLCPP_INFO(rclcpp::get_logger("Map Action"), "Cek new pose speed %f", new_pose.get_speed());
+  RCLCPP_INFO(rclcpp::get_logger("Map Action"), "Cek new pose joints size %ld", new_pose.get_joints().size());
+  for (int joint = new_pose.get_joints().size() - 1; joint >= 0; --joint)
+  {
+    RCLCPP_INFO(rclcpp::get_logger("Map Action"), "Cek new pose joints %d: %d", new_pose.get_joints().at(joint).get_id(), new_pose.get_joints().at(joint).get_position_value());
+  }
+
+  this->delete_pose(target_pose_index);
+  this->set_pose(target_pose_index, new_pose);
+
+  RCLCPP_INFO(rclcpp::get_logger("Map Action"), "Cek changed pose name %s", this->get_pose(target_pose_index).get_name().c_str());
+  RCLCPP_INFO(rclcpp::get_logger("Map Action"), "Cek changed pose pause %f", this->get_pose(target_pose_index).get_pause());
+  RCLCPP_INFO(rclcpp::get_logger("Map Action"), "Cek changed pose speed %f", this->get_pose(target_pose_index).get_speed());
+  RCLCPP_INFO(rclcpp::get_logger("Map Action"), "Cek changed pose joints size %ld", this->get_pose(target_pose_index).get_joints().size());
+  for (int joint = this->get_pose(target_pose_index).get_joints().size() - 1; joint >= 0; --joint)
+  {
+    RCLCPP_INFO(rclcpp::get_logger("Map Action"), "Cek changed pose joints %d: %d", this->get_pose(target_pose_index).get_joints().at(joint).get_id(), this->get_pose(target_pose_index).get_joints().at(joint).get_position_value());
+  }
+}
+
 }  // namespace akushon

src/akushon/action/node/action_manager.cpp

@@ -31,7 +31,7 @@
 #include <vector>
 
 #include "akushon/action/node/action_manager.hpp"
-
+#include "jitsuyo/config.hpp"
 #include "akushon/action/model/action_name.hpp"
 #include "akushon/action/process/interpolator.hpp"
 #include "nlohmann/json.hpp"
@@ -41,7 +41,7 @@ namespace akushon
 {
 
 ActionManager::ActionManager()
-: actions({}), is_running(false)
+: actions({}), is_running(false), config_name("dynamic_kick.json"), action_dir("actions/")
 {
   interpolator = std::make_shared<Interpolator>(std::vector<Action>(), Pose(""));
 }
@@ -67,11 +67,24 @@ Action ActionManager::get_action(std::string action_name) const
 
 void ActionManager::load_config(const std::string & path)
 {
-  for (const auto & entry : std::filesystem::directory_iterator(path)) {
+  try {
+    std::ifstream file(path + config_name);
+    nlohmann::json data = nlohmann::json::parse(file);
+
+    set_config(path);
+
+    file.close();
+  } catch (nlohmann::json::parse_error & ex) {
+      std::cerr << "failed to load action: " << config_name << std::endl;
+      std::cerr << "parse error at byte " << ex.byte << std::endl;
+      throw ex;
+  }
+
+  for (const auto & entry : std::filesystem::directory_iterator(path + action_dir)) {
     std::string name = "";
     std::string file_name = entry.path();
     std::string extension_json = ".json";
-    for (int i = path.length(); i < file_name.length() - extension_json.length(); i++) {
+    for (int i = (path + action_dir).length() - 1; i < file_name.length() - extension_json.length(); ++i) {
       name += file_name[i];
     }
 
@@ -90,6 +103,31 @@ void ActionManager::load_config(const std::string & path)
   }
 }
 
+void ActionManager::set_config(const std::string & path)
+{
+  nlohmann::json config;
+  if (!jitsuyo::load_config(path, "dynamic_kick.json", config)) {
+    throw std::runtime_error("Failed to load config file `" + path + "dynamic_kick.json`");
+  }
+
+  bool valid_config = true;
+
+  nlohmann::json dynamic_kick_section;
+  if (jitsuyo::assign_val(config, "dynamic_kick", dynamic_kick_section)) {
+    bool valid_section = jitsuyo::assign_val(dynamic_kick_section, "right_map_x_min", right_map_x_min);
+    valid_section &= jitsuyo::assign_val(dynamic_kick_section, "right_map_x_max", right_map_x_max);
+    valid_section &= jitsuyo::assign_val(dynamic_kick_section, "left_map_x_min", left_map_x_min);
+    valid_section &= jitsuyo::assign_val(dynamic_kick_section, "left_map_x_max", left_map_x_max);
+    valid_section &= jitsuyo::assign_val(dynamic_kick_section, "using_dynamic_kick", using_dynamic_kick);
+    if (!valid_section) {
+      std::cout << "Error found at section `dynamic_kick`" << std::endl;
+      valid_config = false;
+    }
+  } else {
+    valid_config = false;
+  }
+}
+
 Action ActionManager::load_action(
   const nlohmann::json & action_data,
   const std::string & action_name) const
@@ -160,6 +198,50 @@ void ActionManager::start(std::string action_name, const Pose & initial_pose)
   is_running = true;
 }
 
+void ActionManager::start(std::string action_name, std::string target_action_name, const Pose & initial_pose, float ball_x, float right_map_x_min_, float right_map_x_max_, float left_map_x_min_, float left_map_x_max_, bool right)
+{
+  std::vector<Action> target_actions;
+
+  while (true) {
+    auto action = actions.at(action_name);
+    const auto & target_action = actions.at(target_action_name);
+    int pose_count = action.get_pose_count();
+    if (right)
+    {
+      for (int pose_index = pose_count - 1; pose_index >= 0; --pose_index) {
+        RCLCPP_INFO(rclcpp::get_logger("DEBUG ACTION MANAGER"), "Start Mapping!");
+        action.map_action(action, target_action, pose_index, ball_x, right_map_x_min_, right_map_x_max_);
+      }
+    } else {
+      for (int pose_index = pose_count - 1; pose_index >= 0; --pose_index) {
+        RCLCPP_INFO(rclcpp::get_logger("DEBUG ACTION MANAGER"), "Start Mapping!");
+        action.map_action(action, target_action, pose_index, ball_x, left_map_x_min_, left_map_x_max_);
+      }
+    }
+
+    RCLCPP_INFO(rclcpp::get_logger("DEBUG ACTION MANAGER"), "New Action Count: %d", action.get_pose_count());
+    for (const auto& the_action : target_actions) {
+      RCLCPP_INFO(rclcpp::get_logger("DEBUG ACTION MANAGER"), "Action Vector: %s", the_action.get_name().c_str());
+    }
+    target_actions.push_back(action);
+
+    if (action.get_next_action() != "") {
+      std::string next_action_name = action.get_next_action();
+
+      if (actions.find(next_action_name) != actions.end()) {
+        action_name = next_action_name;
+      } else {
+        break;
+      }
+    } else {
+      break;
+    }
+  }
+
+  interpolator = std::make_shared<Interpolator>(target_actions, initial_pose);
+  is_running = true;
+}
+
 void ActionManager::start(const Action & action, const Pose & initial_pose)
 {
   std::vector<Action> target_actions {action};

src/akushon/action/node/action_node.cpp

@@ -45,6 +45,8 @@ std::string ActionNode::brake_action_topic() {return get_node_prefix() + "/brake
 
 std::string ActionNode::status_topic() {return get_node_prefix() + "/status";}
 
+std::string ActionNode::ball_topic() {return get_node_prefix() + "/ball";}
+
 ActionNode::ActionNode(
   rclcpp::Node::SharedPtr node, std::shared_ptr<ActionManager> & action_manager)
 : node(node), action_manager(action_manager), initial_pose(Pose("initial_pose"))
@@ -88,14 +90,33 @@ ActionNode::ActionNode(
   brake_action_subscriber = node->create_subscription<Empty>(
     brake_action_topic(), 10,
     [this](std::shared_ptr<Empty> message) {this->action_manager->brake();});
+
+  ball_subscriber = node->create_subscription<Float64>(
+    ball_topic(), 10, [this](const std::shared_ptr<Float64> message) {
+      ball_pos = message->data;
+    });
 }
 
 bool ActionNode::start(const std::string & action_name)
 {
   Pose pose = this->initial_pose;
 
   if (!pose.get_joints().empty()) {
-    action_manager->start(action_name, pose);
+    if (action_name == akushon::ActionName::LEFT_KICK_WIDE || action_name == akushon::ActionName::RIGHT_KICK_WIDE)
+    {
+      std::size_t pos = action_name.find("_wide");
+      std::string source_action = pos != std::string::npos ? action_name.substr(0, pos) : action_name;
+
+      RCLCPP_INFO(rclcpp::get_logger("Action Node"), "Start WIDE_KICK, source %s, target %s\n", source_action.c_str(), action_name.c_str());
+      if (action_manager->using_dynamic_kick)
+        action_manager->start(source_action, action_name, pose, ball_pos, action_manager->right_map_x_min, action_manager->right_map_x_max, action_manager->left_map_x_min, action_manager->left_map_x_max, source_action == "right_kick");
+      else
+        action_manager->start(action_name, pose);  
+    }
+    else {
+      RCLCPP_INFO(rclcpp::get_logger("Action Node"), "NOT WIDE KICK\n");
+      action_manager->start(action_name, pose);
+    }
   } else {
     return false;
   }

src/akushon/config/node/config_node.cpp

@@ -51,7 +51,7 @@ ConfigNode::ConfigNode(rclcpp::Node::SharedPtr node, const std::string & path,
       response->status = "SAVED";
     }
   );
-  config_grpc.Run(5060, path, node, action_manager);
+  config_grpc.Run(5050, path, node, action_manager);
   RCLCPP_INFO(rclcpp::get_logger("GrpcServers"), "grpc running");
 }