fix: fix map action, fix start action

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
@@ -114,14 +114,43 @@ void Action::map_action(const Action & source_action, const Action & target_acti
 {
   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 = 0; joint < 20; joint++)
+  for (int joint = 0; joint < int(src_joints.size()); 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);
-      src_joints[joint].set_position_value(new_joint_value);
+      target_joints[joint].set_position_value(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 = 0; joint < int(new_pose.get_joints().size()); 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 = 0; joint < int(this->get_pose(target_pose_index).get_joints().size()); 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());
   }
 }
 

src/akushon/action/node/action_manager.cpp

@@ -197,22 +197,26 @@ void ActionManager::start(std::string action_name, std::string target_action_nam
   std::vector<Action> target_actions;
 
   while (true) {
-    auto & action = actions.at(action_name);
+    auto action = actions.at(action_name);
     const auto & target_action = actions.at(target_action_name);
-
-    for (int pose_index = 0; pose_index < action.get_pose_count(); pose_index++) {
+    int pose_count = action.get_pose_count();
+    for (int pose_index = 0; pose_index < pose_count; pose_index++) {
       if (right)
       {
-        RCLCPP_INFO(rclcpp::get_logger("DEBUG ACTION MANAGER"), "CEK1");
+        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 {
-        RCLCPP_INFO(rclcpp::get_logger("DEBUG ACTION MANAGER"), "CEK1");
+        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);
-    RCLCPP_INFO(rclcpp::get_logger("DEBUG ACTION MANAGER"), "CEK");
+
     if (action.get_next_action() != "") {
       std::string next_action_name = action.get_next_action();
 

src/akushon/action/node/action_node.cpp

@@ -117,12 +117,15 @@ bool ActionNode::start(const std::string & action_name)
       if (source_action == "right_kick") {
         right = true;
       }
+
+      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, right);
       else
-        action_manager->start(source_action, pose);  
+        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 {

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