From dd0e3902795f009dc648962a1797bdaf25d3c366 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Sun, 13 Oct 2024 05:14:42 +0900
Subject: [PATCH] =?UTF-8?q?KickState::CHASE=5FBALL=E3=81=A7=E3=81=AF?=
 =?UTF-8?q?=E3=83=9C=E3=83=BC=E3=83=AB=E5=9B=9E=E9=81=BF=E3=82=92ON=20/=20?=
 =?UTF-8?q?=20KickState::POSITIVE=5FREDIRECT=20(#606)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../include/crane_robot_skills/kick.hpp       | 50 +++++++++++++++++++
 crane_robot_skills/src/attacker.cpp           |  2 +-
 2 files changed, 51 insertions(+), 1 deletion(-)

diff --git a/crane_robot_skills/include/crane_robot_skills/kick.hpp b/crane_robot_skills/include/crane_robot_skills/kick.hpp
index 26025eff..bab4da60 100644
--- a/crane_robot_skills/include/crane_robot_skills/kick.hpp
+++ b/crane_robot_skills/include/crane_robot_skills/kick.hpp
@@ -20,6 +20,7 @@ enum class KickState {
   AROUND_BALL,
   KICK,
   REDIRECT_KICK,
+  POSITIVE_REDIRECT_KICK,
 };
 
 class Kick : public SkillBaseWithState<KickState, RobotCommandWrapperPosition>
@@ -58,6 +59,7 @@ class Kick : public SkillBaseWithState<KickState, RobotCommandWrapperPosition>
         visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::ENTRY_POINT");
         return Status::RUNNING;
       });
+
     addTransition(KickState::ENTRY_POINT, KickState::CHASE_BALL, [this]() {
       return world_model()->ball.isMoving(getParameter<double>("moving_speed_threshold"));
     });
@@ -81,20 +83,68 @@ class Kick : public SkillBaseWithState<KickState, RobotCommandWrapperPosition>
           command.setTargetPosition(ball_exit_point).lookAtBallFrom(ball_exit_point);
           state << "ball_exit: " << ball_exit_point.x() << ", " << ball_exit_point.y();
         }
+        command.enableBallAvoidance();
         visualizer->addPoint(robot()->pose.pos, 0, "", 1., state.str());
         return Status::RUNNING;
       });
 
     addTransition(KickState::CHASE_BALL, KickState::AROUND_BALL, [this]() {
       // ボールが止まったら回り込みへ
+      command.disableBallAvoidance();
       return not world_model()->ball.isMoving(getParameter<double>("moving_speed_threshold"));
     });
 
     addTransition(KickState::CHASE_BALL, KickState::REDIRECT_KICK, [this]() {
       // ボールライン上に乗ったらリダイレクトキックへ
+      command.disableBallAvoidance();
+      return world_model()->ball.isMovingTowards(robot()->pose.pos, 10.0) &&
+             getAngleDiff(
+               getAngle(world_model()->ball.vel),
+               getAngle(getParameter<Point>("target") - robot()->pose.pos) < M_PI / 2.0);
+    });
+
+    addTransition(KickState::CHASE_BALL, KickState::POSITIVE_REDIRECT_KICK, [this]() {
+      command.disableBallAvoidance();
       return world_model()->ball.isMovingTowards(robot()->pose.pos, 10.0);
     });
 
+    addStateFunction(
+      KickState::POSITIVE_REDIRECT_KICK,
+      [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) {
+        visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::POSITIVE_REDIRECT_KICK");
+        // ボールラインに沿って追いかけつつ、角度はtargetへ向ける
+        const auto & ball_pos = world_model()->ball.pos;
+        command.lookAtFrom(getParameter<Point>("target"), ball_pos);
+
+        const auto & ball_vel_normed = world_model()->ball.vel.normalized();
+        Segment ball_line{ball_pos - ball_vel_normed * 10, ball_pos + ball_vel_normed * 10};
+        auto [distance, closest_point] = getClosestPointAndDistance(ball_pos, ball_line);
+        if ((ball_pos - closest_point).dot(ball_vel_normed) > 0) {
+          // 通り過ぎていれば追いかけて蹴る
+          auto target_pos = [&]() -> Point {
+            if (distance < 0.1) {
+              return ball_pos + ball_vel_normed;
+            } else {
+              return closest_point + ball_vel_normed * distance;
+            }
+          }();
+          command.setDribblerTargetPosition(target_pos);
+          command.kickStraight(0.3);
+          command.disableBallAvoidance();
+        } else {
+          // まだだったら避ける
+          command.setTargetPosition(
+            closest_point + (robot()->pose.pos - closest_point).normalized() * 0.3);
+        }
+
+        return Status::RUNNING;
+      });
+
+    addTransition(KickState::POSITIVE_REDIRECT_KICK, KickState::ENTRY_POINT, [this]() {
+      return !world_model()->ball.isMovingAwayFrom(robot()->pose.pos, 10.0) or
+             !world_model()->ball.isMovingTowards(getParameter<Point>("target"), 30.0);
+    });
+
     addStateFunction(
       KickState::REDIRECT_KICK, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) {
         visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::REDIRECT_KICK");
diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp
index 82ccba5e..96877875 100644
--- a/crane_robot_skills/src/attacker.cpp
+++ b/crane_robot_skills/src/attacker.cpp
@@ -97,7 +97,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
     AttackerState::CUT_THEIR_PASS,
     [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
       visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 0.5);
-      receive_skill.setParameter("enable_redirect", true);
+      receive_skill.setParameter("enable_redirect", false);
       receive_skill.setParameter("policy", std::string("min_slack"));
       return receive_skill.run(visualizer);
     });