diff --git a/include/akushon/action/model/action.hpp b/include/akushon/action/model/action.hpp index ea1d222..03ec5cc 100644 --- a/include/akushon/action/model/action.hpp +++ b/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; diff --git a/include/akushon/action/node/action_manager.hpp b/include/akushon/action/node/action_manager.hpp index 46a7872..21467d9 100644 --- a/include/akushon/action/node/action_manager.hpp +++ b/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 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 actions; std::shared_ptr interpolator; bool is_running; + std::string config_name; + std::string action_dir; }; } // namespace akushon diff --git a/include/akushon/action/node/action_node.hpp b/include/akushon/action/node/action_node.hpp index ad8ea87..053333a 100644 --- a/include/akushon/action/node/action_node.hpp +++ b/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 & action_manager); @@ -68,6 +72,8 @@ class ActionNode void publish_status(); Pose initial_pose; + double ball_pos; + rclcpp::Node::SharedPtr node; std::shared_ptr action_manager; @@ -77,6 +83,7 @@ class ActionNode rclcpp::Subscription::SharedPtr run_action_subscriber; rclcpp::Subscription::SharedPtr brake_action_subscriber; + rclcpp::Subscription::SharedPtr ball_subscriber; rclcpp::Publisher::SharedPtr status_publisher; }; diff --git a/include/akushon/config/utils/config.hpp b/include/akushon/config/utils/config.hpp index 20e53d5..4d08ef0 100644 --- a/include/akushon/config/utils/config.hpp +++ b/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 diff --git a/src/akushon/action/model/action.cpp b/src/akushon/action/model/action.cpp index c6e23fd..d7808d1 100644 --- a/src/akushon/action/model/action.cpp +++ b/src/akushon/action/model/action.cpp @@ -24,7 +24,7 @@ #include #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 src_joints = source_action.get_pose(target_pose_index).get_joints(); + std::vector 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(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 diff --git a/src/akushon/action/node/action_manager.cpp b/src/akushon/action/node/action_manager.cpp index 9b8b727..8e0b8a3 100644 --- a/src/akushon/action/node/action_manager.cpp +++ b/src/akushon/action/node/action_manager.cpp @@ -31,7 +31,7 @@ #include #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(std::vector(), 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 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(target_actions, initial_pose); + is_running = true; +} + void ActionManager::start(const Action & action, const Pose & initial_pose) { std::vector target_actions {action}; diff --git a/src/akushon/action/node/action_node.cpp b/src/akushon/action/node/action_node.cpp index 8755604..b994f42 100755 --- a/src/akushon/action/node/action_node.cpp +++ b/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 & action_manager) : node(node), action_manager(action_manager), initial_pose(Pose("initial_pose")) @@ -88,6 +90,11 @@ ActionNode::ActionNode( brake_action_subscriber = node->create_subscription( brake_action_topic(), 10, [this](std::shared_ptr message) {this->action_manager->brake();}); + + ball_subscriber = node->create_subscription( + ball_topic(), 10, [this](const std::shared_ptr message) { + ball_pos = message->data; + }); } bool ActionNode::start(const std::string & action_name) @@ -95,7 +102,21 @@ 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; } diff --git a/src/akushon/config/node/config_node.cpp b/src/akushon/config/node/config_node.cpp index 553bfcc..b6a58fe 100644 --- a/src/akushon/config/node/config_node.cpp +++ b/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"); } diff --git a/src/akushon/config/utils/config.cpp b/src/akushon/config/utils/config.cpp index 66f1223..b861ffd 100644 --- a/src/akushon/config/utils/config.cpp +++ b/src/akushon/config/utils/config.cpp @@ -32,7 +32,7 @@ namespace akushon { Config::Config(const std::string & path) -: path(path) +: path(path), action_dir("actions/") { } @@ -40,7 +40,7 @@ std::string Config::get_config() const { nlohmann::json actions_list; std::cout << "[ ACTIONS LIST ] : " << std::endl; - for (const auto & action_file : std::filesystem::directory_iterator(path)) { + for (const auto & action_file : std::filesystem::directory_iterator(path + action_dir)) { std::string file_name = action_file.path(); std::string action_name = ""; @@ -56,7 +56,7 @@ std::string Config::get_config() const continue; } - if (action_data["name"] != action_name) { + if (action_data["name"] != action_name.substr(8)) { std::cerr << "Action name does not match file name in " << file_name << std::endl; continue; } @@ -74,19 +74,36 @@ std::string Config::get_config() const void Config::save_config(const std::string & actions_data) { + clear_directory(path + action_dir); nlohmann::json actions_list = nlohmann::json::parse(actions_data); for (const auto & [key, val] : actions_list.items()) { std::locale loc; std::string action_name = key; std::replace(action_name.begin(), action_name.end(), ' ', '_'); - std::string file_name = path + action_name + ".json"; + std::string file_name = path + action_dir + action_name + ".json"; std::ofstream file; - if (!jitsuyo::save_config(path, action_name + ".json", val)) { + if (!jitsuyo::save_config(path + action_dir, action_name + ".json", val)) { std::cerr << "Failed to save " << file_name << std::endl; continue; } } } +void Config::clear_directory(const std::string& directory_path) +{ + try { + if (std::filesystem::exists(directory_path) && std::filesystem::is_directory(directory_path)) { + for (const auto& entry : std::filesystem::directory_iterator(directory_path)) { + std::filesystem::remove_all(entry.path()); + } + std::cout << "Directory cleared: " << directory_path << std::endl; + } else { + std::cerr << "Directory does not exist: " << directory_path << std::endl; + } + } catch (const std::filesystem::filesystem_error& e) { + std::cerr << "Error clearing directory: " << e.what() << std::endl; + } +} + } // namespace akushon