Merge pull request #151 from ibis-ssl/feature/aggressive_goalie

オーバーシュートを恐れないGoalie
ibis-ssl · Feb 23, 2024 · 0d9d646 · 0d9d646
2 parents 14bc3fc + d0a532b
commit 0d9d646
Show file tree

Hide file tree

Showing 10 changed files with 166 additions and 270 deletions.
diff --git a/crane_robot_skills/include/crane_robot_skills/goalie.hpp b/crane_robot_skills/include/crane_robot_skills/goalie.hpp
@@ -20,95 +20,9 @@ class Goalie : public SkillBase<>
 public:
   explicit Goalie(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm);
 
-  void emitBallFromPenaltyArea(crane::RobotCommandWrapper::SharedPtr & command)
-  {
-    auto ball = world_model->ball.pos;
-    // パスできるロボットのリストアップ
-    auto passable_robot_list = world_model->ours.getAvailableRobots(command->robot->id);
-    passable_robot_list.erase(
-      std::remove_if(
-        passable_robot_list.begin(), passable_robot_list.end(),
-        [&](const RobotInfo::SharedPtr & r) {
-          // 敵に塞がれていたら除外
-          Segment ball_to_robot_line(ball, r->pose.pos);
-          for (const auto & enemy : world_model->theirs.getAvailableRobots()) {
-            auto dist = bg::distance(ball_to_robot_line, enemy->pose.pos);
-            if (dist < 0.2) {
-              return true;
-            }
-          }
-          return false;
-        }),
-      passable_robot_list.end());
+  void emitBallFromPenaltyArea(crane::RobotCommandWrapper::SharedPtr & command);
 
-    Point pass_target = [&]() {
-      if (not passable_robot_list.empty()) {
-        // TODO(HansRobo): いい感じのロボットを選ぶようにする
-        return passable_robot_list.front()->pose.pos;
-      } else {
-        return Point{0, 0};
-      }
-    }();
-
-    Point intermediate_point = ball + (ball - pass_target).normalized() * 0.2f;
-    double angle_ball_to_target = getAngle(pass_target - ball);
-    double dot = (world_model->ball.pos - command->robot->pose.pos)
-                   .normalized()
-                   .dot((pass_target - world_model->ball.pos).normalized());
-    // ボールと目標の延長線上にいない && 角度があってないときは，中間ポイントを経由
-    if (
-      dot < 0.95 ||
-      std::abs(getAngleDiff(angle_ball_to_target, command->robot->pose.theta)) > 0.05) {
-      command->setTargetPosition(intermediate_point);
-      command->enableCollisionAvoidance();
-    } else {
-      command->setTargetPosition(world_model->ball.pos);
-      command->kickWithChip(0.4).disableCollisionAvoidance();
-      command->enableCollisionAvoidance();
-      command->disableBallAvoidance();
-    }
-    command->setTargetTheta(getAngle(pass_target - ball));
-    command->disableGoalAreaAvoidance();
-  }
-
-  void inplay(crane::RobotCommandWrapper::SharedPtr & command, bool enable_emit = true)
-  {
-    auto goals = world_model->getOurGoalPosts();
-    auto ball = world_model->ball.pos;
-    // シュートチェック
-    Segment goal_line(goals.first, goals.second);
-    Segment ball_line(ball, ball + world_model->ball.vel.normalized() * 20.f);
-    std::vector<Point> intersections;
-    bg::intersection(ball_line, Segment{goals.first, goals.second}, intersections);
-    if (not intersections.empty() && world_model->ball.vel.norm() > 0.5f) {
-      // シュートブロック
-      phase = "シュートブロック";
-      ClosestPoint result;
-      bg::closest_point(ball_line, command->robot->pose.pos, result);
-      command->setTargetPosition(result.closest_point);
-      command->setTargetTheta(getAngle(-world_model->ball.vel));
-    } else {
-      if (world_model->ball.isStopped() && world_model->isFriendDefenseArea(ball) && enable_emit) {
-        // ボールが止まっていて，味方ペナルティエリア内にあるときは，ペナルティエリア外に出す
-        phase = "ボール排出";
-        emitBallFromPenaltyArea(command);
-      } else {
-        phase = "";
-        const double BLOCK_DIST = 0.15;
-        // 範囲外のときは正面に構える
-        if (not world_model->isFieldInside(ball)) {
-          phase += "正面で";
-          ball << 0, 0;
-        }
-
-        phase = "ボールを待ち受ける";
-        Point goal_center = world_model->getOurGoalCenter();
-        goal_center << goals.first.x(), 0.0f;
-        command->setTargetPosition(goal_center + (ball - goal_center).normalized() * BLOCK_DIST);
-        command->lookAtBall();
-      }
-    }
-  }
+  void inplay(crane::RobotCommandWrapper::SharedPtr & command, bool enable_emit = true);
 
   void print(std::ostream & os) const override { os << "[Goalie] " << phase; }
 

diff --git a/crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp b/crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp
@@ -56,11 +56,16 @@ class PenaltyKick : public SkillBase<PenaltyKickState>
           start_ball_point = world_model->ball.pos;
         }
 
-        auto [best_target, goal_angle_width] = getBestShootTargetWithWidth();
+        auto [best_angle, goal_angle_width] =
+          world_model->getLargestGoalAngleRangeFromPoint(world_model->ball.pos);
+        Point best_target = world_model->ball.pos + getNormVec(best_angle) * 0.5;
+        visualizer->addPoint(best_target.x(), best_target.y(), 1, "red", 1.0, "best_target");
 
         // 経由ポイント
         Point intermediate_point =
           world_model->ball.pos + (world_model->ball.pos - best_target).normalized() * 0.2;
+        visualizer->addPoint(
+          intermediate_point.x(), intermediate_point.y(), 1, "red", 1.0, "intermediate_point");
 
         double dot = (robot->pose.pos - world_model->ball.pos)
                        .normalized()
@@ -92,35 +97,6 @@ class PenaltyKick : public SkillBase<PenaltyKickState>
       });
   }
 
-  // SimpleAttackerからコピー
-  auto getBestShootTargetWithWidth() -> std::pair<Point, double>
-  {
-    const auto & ball = world_model->ball.pos;
-
-    Interval goal_range;
-
-    auto goal_posts = world_model->getTheirGoalPosts();
-    goal_range.append(getAngle(goal_posts.first - ball), getAngle(goal_posts.second - ball));
-
-    for (auto & enemy : world_model->theirs.getAvailableRobots()) {
-      double distance = enemy->getDistance(ball);
-      constexpr double MACHINE_RADIUS = 0.1;
-
-      double center_angle = getAngle(enemy->pose.pos - ball);
-      double diff_angle =
-        atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS));
-
-      goal_range.erase(center_angle - diff_angle, center_angle + diff_angle);
-    }
-
-    auto largest_interval = goal_range.getLargestInterval();
-
-    double target_angle = (largest_interval.first + largest_interval.second) / 2.0;
-
-    return {
-      ball + getNormVec(target_angle) * 0.5, largest_interval.second - largest_interval.first};
-  }
-
   void print(std::ostream & os) const override
   {
     os << "[Idle] stop_by_position: " << getParameter<bool>("stop_by_position") ? "true" : "false";

diff --git a/crane_robot_skills/include/crane_robot_skills/receiver.hpp b/crane_robot_skills/include/crane_robot_skills/receiver.hpp
@@ -44,50 +44,6 @@ class Receiver : public SkillBase<>
 
   void print(std::ostream & os) const override { os << "[Receiver]"; }
 
-  auto getLargestGoalAngleWidthFromPosition(const Point point) -> double
-  {
-    Interval goal_range;
-
-    auto goal_posts = world_model->getTheirGoalPosts();
-    goal_range.append(getAngle(goal_posts.first - point), getAngle(goal_posts.second - point));
-
-    for (auto & enemy : world_model->theirs.robots) {
-      double distance = (point - enemy->pose.pos).norm();
-      constexpr double MACHINE_RADIUS = 0.1;
-
-      double center_angle = getAngle(enemy->pose.pos - point);
-      double diff_angle =
-        atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS));
-
-      goal_range.erase(center_angle - diff_angle, center_angle + diff_angle);
-    }
-
-    auto largest_interval = goal_range.getLargestInterval();
-    return largest_interval.second - largest_interval.first;
-  }
-
-  auto getLargestGoalAngleFromPosition(const Point point) -> double
-  {
-    Interval goal_range;
-
-    auto goal_posts = world_model->getTheirGoalPosts();
-    goal_range.append(getAngle(goal_posts.first - point), getAngle(goal_posts.second - point));
-
-    for (auto & enemy : world_model->theirs.robots) {
-      double distance = (point - enemy->pose.pos).norm();
-      constexpr double MACHINE_RADIUS = 0.1;
-
-      double center_angle = getAngle(enemy->pose.pos - point);
-      double diff_angle =
-        atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS));
-
-      goal_range.erase(center_angle - diff_angle, center_angle + diff_angle);
-    }
-
-    auto largest_interval = goal_range.getLargestInterval();
-    return (largest_interval.second + largest_interval.first) * 0.5;
-  }
-
   std::vector<std::pair<double, Point>> getPositionsWithScore(Segment ball_line, Point next_target)
   {
     auto points = getPoints(ball_line, 0.05);

diff --git a/crane_robot_skills/include/crane_robot_skills/simple_attacker.hpp b/crane_robot_skills/include/crane_robot_skills/simple_attacker.hpp
@@ -24,34 +24,6 @@ class SimpleAttacker : public SkillBase<>
   {
     os << "[Idle] stop_by_position: " << getParameter<bool>("stop_by_position") ? "true" : "false";
   }
-
-  auto getBestShootTargetWithWidth() -> std::pair<Point, double>
-  {
-    const auto & ball = world_model->ball.pos;
-
-    Interval goal_range;
-
-    auto goal_posts = world_model->getTheirGoalPosts();
-    goal_range.append(getAngle(goal_posts.first - ball), getAngle(goal_posts.second - ball));
-
-    for (auto & enemy : world_model->theirs.robots) {
-      double distance = (ball - enemy->pose.pos).norm();
-      constexpr double MACHINE_RADIUS = 0.1;
-
-      double center_angle = getAngle(enemy->pose.pos - ball);
-      double diff_angle =
-        atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS));
-
-      goal_range.erase(center_angle - diff_angle, center_angle + diff_angle);
-    }
-
-    auto largest_interval = goal_range.getLargestInterval();
-
-    double target_angle = (largest_interval.first + largest_interval.second) / 2.0;
-
-    return {
-      ball + getNormVec(target_angle) * 0.5, largest_interval.second - largest_interval.first};
-  }
 };
 }  // namespace crane::skills
 #endif  // CRANE_ROBOT_SKILLS__SIMPLE_ATTACKER_HPP_
diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp
@@ -36,4 +36,97 @@ Goalie::Goalie(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)
       return Status::RUNNING;
     });
 }
+
+void Goalie::emitBallFromPenaltyArea(RobotCommandWrapper::SharedPtr & command)
+{
+  auto ball = world_model->ball.pos;
+  // パスできるロボットのリストアップ
+  auto passable_robot_list = world_model->ours.getAvailableRobots(command->robot->id);
+  passable_robot_list.erase(
+    std::remove_if(
+      passable_robot_list.begin(), passable_robot_list.end(),
+      [&](const RobotInfo::SharedPtr & r) {
+        // 敵に塞がれていたら除外
+        Segment ball_to_robot_line(ball, r->pose.pos);
+        for (const auto & enemy : world_model->theirs.getAvailableRobots()) {
+          auto dist = bg::distance(ball_to_robot_line, enemy->pose.pos);
+          if (dist < 0.2) {
+            return true;
+          }
+        }
+        return false;
+      }),
+    passable_robot_list.end());
+
+  Point pass_target = [&]() {
+    if (not passable_robot_list.empty()) {
+      // TODO(HansRobo): いい感じのロボットを選ぶようにする
+      return passable_robot_list.front()->pose.pos;
+    } else {
+      return Point{0, 0};
+    }
+  }();
+
+  Point intermediate_point = ball + (ball - pass_target).normalized() * 0.2f;
+  double angle_ball_to_target = getAngle(pass_target - ball);
+  double dot = (world_model->ball.pos - command->robot->pose.pos)
+                 .normalized()
+                 .dot((pass_target - world_model->ball.pos).normalized());
+  // ボールと目標の延長線上にいない && 角度があってないときは，中間ポイントを経由
+  if (
+    dot < 0.95 || std::abs(getAngleDiff(angle_ball_to_target, command->robot->pose.theta)) > 0.05) {
+    command->setTargetPosition(intermediate_point);
+    command->enableCollisionAvoidance();
+  } else {
+    command->setTargetPosition(world_model->ball.pos);
+    command->kickWithChip(0.4).disableCollisionAvoidance();
+    command->enableCollisionAvoidance();
+    command->disableBallAvoidance();
+  }
+  command->setTargetTheta(getAngle(pass_target - ball));
+  command->disableGoalAreaAvoidance();
+}
+
+void Goalie::inplay(RobotCommandWrapper::SharedPtr & command, bool enable_emit)
+{
+  auto goals = world_model->getOurGoalPosts();
+  auto ball = world_model->ball.pos;
+  // シュートチェック
+  Segment goal_line(goals.first, goals.second);
+  Segment ball_line(ball, ball + world_model->ball.vel.normalized() * 20.f);
+  std::vector<Point> intersections;
+  bg::intersection(ball_line, Segment{goals.first, goals.second}, intersections);
+  command->setTerminalVelocity(0.0);
+  if (not intersections.empty() && world_model->ball.vel.norm() > 0.5f) {
+    // シュートブロック
+    phase = "シュートブロック";
+    ClosestPoint result;
+    bg::closest_point(ball_line, command->robot->pose.pos, result);
+    command->setTargetPosition(result.closest_point);
+    command->setTargetTheta(getAngle(-world_model->ball.vel));
+    if (command->robot->getDistance(ball) > 0.3) {
+      command->setTerminalVelocity(2.0);
+    }
+  } else {
+    if (world_model->ball.isStopped() && world_model->isFriendDefenseArea(ball) && enable_emit) {
+      // ボールが止まっていて，味方ペナルティエリア内にあるときは，ペナルティエリア外に出す
+      phase = "ボール排出";
+      emitBallFromPenaltyArea(command);
+    } else {
+      phase = "";
+      const double BLOCK_DIST = 0.15;
+      // 範囲外のときは正面に構える
+      if (not world_model->isFieldInside(ball)) {
+        phase += "正面で";
+        ball << 0, 0;
+      }
+
+      phase = "ボールを待ち受ける";
+      Point goal_center = world_model->getOurGoalCenter();
+      goal_center << goals.first.x(), 0.0f;
+      command->setTargetPosition(goal_center + (ball - goal_center).normalized() * BLOCK_DIST);
+      command->lookAtBall();
+    }
+  }
+}
 }  // namespace crane::skills
diff --git a/crane_robot_skills/src/receiver.cpp b/crane_robot_skills/src/receiver.cpp
@@ -43,7 +43,8 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)
 
           // ゴールとボールの中間方向を向く
           // TODO(Hansobo): ボールの速さ・キッカーの強さでボールの反射する角度が変わるため、要考慮
-          auto goal_angle = getLargestGoalAngleFromPosition(result.closest_point);
+          auto [goal_angle, width] =
+            world_model->getLargestGoalAngleRangeFromPoint(result.closest_point);
           auto to_goal = getNormVec(goal_angle);
           auto to_ball = (world_model->ball.pos - result.closest_point).normalized();
           double intermediate_angle = getAngle(2 * to_goal + to_ball);
@@ -74,7 +75,8 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)
             continue;
           }
 
-          double score = getLargestGoalAngleWidthFromPosition(dpps_point);
+          auto [goal_angle, width] = world_model->getLargestGoalAngleRangeFromPoint(dpps_point);
+          double score = width;
           const double dist = (robot->pose.pos - dpps_point).norm();
           score = score * (1.0 - dist / 10.0);
 
@@ -89,7 +91,7 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)
 
       // ゴールとボールの中間方向を向く
       Point target_pos{command->latest_msg.target_x.front(), command->latest_msg.target_y.front()};
-      auto goal_angle = getLargestGoalAngleFromPosition(target_pos);
+      auto [goal_angle, width] = world_model->getLargestGoalAngleRangeFromPoint(target_pos);
       auto to_goal = getNormVec(goal_angle);
       auto to_ball = (world_model->ball.pos - target_pos).normalized();
       command->setTargetTheta(getAngle(to_goal + to_ball));

diff --git a/crane_robot_skills/src/simple_attacker.cpp b/crane_robot_skills/src/simple_attacker.cpp
@@ -13,7 +13,9 @@ SimpleAttacker::SimpleAttacker(uint8_t id, const std::shared_ptr<WorldModelWrapp
 {
   addStateFunction(
     DefaultStates::DEFAULT, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
-      auto [best_target, goal_angle_width] = getBestShootTargetWithWidth();
+      auto [best_angle, goal_angle_width] =
+        world_model->getLargestGoalAngleRangeFromPoint(world_model->ball.pos);
+      Point best_target = world_model->ball.pos + getNormVec(best_angle) * 0.5;
 
       // シュートの隙がないときは仲間へパス
       if (goal_angle_width < 0.07) {