From 5975a2dfd4ce694d9da8a2c8e0669bdd3a67030e Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Tue, 20 Feb 2024 08:26:09 +0900
Subject: [PATCH] =?UTF-8?q?GoalieSkillPlanner=E3=81=A7=E8=89=B2=E3=80=85?=
 =?UTF-8?q?=E3=81=A1=E3=82=83=E3=82=93=E3=81=A8=E5=88=9D=E6=9C=9F=E5=8C=96?=
 =?UTF-8?q?=E3=81=97=E3=81=9F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../crane_planner_plugins/skill_planner.hpp   | 87 ++++++++++++++++++-
 1 file changed, 86 insertions(+), 1 deletion(-)

diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp
index 3299e89a4..d75d3a7ed 100644
--- a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp
+++ b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp
@@ -57,6 +57,91 @@ namespace crane
     }                                                                                             \
   }
 
-DEFINE_SKILL_PLANNER(Goalie);
+class GoalieSkillPlanner : public PlannerBase
+{
+public:
+  std ::shared_ptr<skills::Goalie> skill = nullptr;
+
+  std ::shared_ptr<RobotCommandWrapper> robot_command_wrapper = nullptr;
+
+  COMPOSITION_PUBLIC explicit GoalieSkillPlanner(
+    WorldModelWrapper ::SharedPtr & world_model, ConsaiVisualizerWrapper ::SharedPtr visualizer)
+  : PlannerBase("Goalie", world_model, visualizer)
+  {
+  }
+
+  std ::pair<Status, std ::vector<crane_msgs ::msg ::RobotCommand>> calculateRobotCommand(
+    const std ::vector<RobotIdentifier> & robots) override
+  {
+    if (not skill or not robot_command_wrapper) {
+      return {PlannerBase ::Status ::RUNNING, {}};
+    } else {
+      std ::vector<crane_msgs ::msg ::RobotCommand> robot_commands;
+      auto status = skill->run(*robot_command_wrapper, visualizer);
+      return {static_cast<PlannerBase ::Status>(status), {robot_command_wrapper->getMsg()}};
+    }
+  }
+  auto getSelectedRobots(
+    uint8_t selectable_robots_num, const std ::vector<uint8_t> & selectable_robots)
+    -> std ::vector<uint8_t> override
+  {
+    skill = std ::make_shared<skills ::Goalie>(world_model->getOurGoalieId(), world_model);
+    robot_command_wrapper =
+      std ::make_shared<RobotCommandWrapper>(world_model->getOurGoalieId(), world_model);
+    return {world_model->getOurGoalieId()};
+  }
+};
+
+class BallPlacementSkillPlanner : public PlannerBase
+{
+public:
+  std ::shared_ptr<skills::SingleBallPlacement> skill = nullptr;
+
+  std ::shared_ptr<RobotCommandWrapper> robot_command_wrapper = nullptr;
+
+  COMPOSITION_PUBLIC explicit BallPlacementSkillPlanner(
+    WorldModelWrapper ::SharedPtr & world_model, ConsaiVisualizerWrapper ::SharedPtr visualizer)
+  : PlannerBase("BallPlacement", world_model, visualizer)
+  {
+  }
+
+  std ::pair<Status, std ::vector<crane_msgs ::msg ::RobotCommand>> calculateRobotCommand(
+    const std ::vector<RobotIdentifier> & robots) override
+  {
+    if (not skill or not robot_command_wrapper) {
+      return {PlannerBase ::Status ::RUNNING, {}};
+    } else {
+      if (auto target = world_model->getBallPlacementTarget(); target.has_value()) {
+        skill->setParameter("placement_x", target->x());
+        skill->setParameter("placement_y", target->y());
+      }
+      std ::vector<crane_msgs ::msg ::RobotCommand> robot_commands;
+      auto status = skill->run(*robot_command_wrapper, visualizer);
+      return {static_cast<PlannerBase ::Status>(status), {robot_command_wrapper->getMsg()}};
+    }
+  }
+
+  auto getSelectedRobots(
+    uint8_t selectable_robots_num, const std ::vector<uint8_t> & selectable_robots)
+    -> std ::vector<uint8_t> override
+  {
+    // ボールに近いロボットを1台選択
+    auto selected_robots = this->getSelectedRobotsByScore(
+      1, selectable_robots, [this](const std::shared_ptr<RobotInfo> & robot) {
+        // ボールに近いほどスコアが高い
+        return 100.0 / std::max(world_model->getSquareDistanceFromRobotToBall(robot->id), 0.01);
+      });
+    skill = std ::make_shared<skills ::SingleBallPlacement>(selected_robots.front(), world_model);
+
+    if (auto target = world_model->getBallPlacementTarget(); target.has_value()) {
+      skill->setParameter("placement_x", target->x());
+      skill->setParameter("placement_y", target->y());
+    }
+    robot_command_wrapper =
+      std ::make_shared<RobotCommandWrapper>(selected_robots.front(), world_model);
+    return {selected_robots.front()};
+  }
+};
+
 }  // namespace crane
 #endif  // CRANE_PLANNER_PLUGINS__SKILL_PLANNER_HPP_