diff --git a/crane_robot_skills/include/crane_robot_skills/goalie.hpp b/crane_robot_skills/include/crane_robot_skills/goalie.hpp index 82d2c0633..76db6a537 100644 --- 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 & 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 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 index 8078f1f27..b74d76829 100644 --- 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 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 }); } - // SimpleAttackerからコピー - auto getBestShootTargetWithWidth() -> std::pair - { - 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("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 index 116c23612..dd081a331 100644 --- 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> 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 index 5298af496..4d91b84a2 100644 --- 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("stop_by_position") ? "true" : "false"; } - - auto getBestShootTargetWithWidth() -> std::pair - { - 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 index 84b2dcf21..b6d8808c3 100644 --- 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 & 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 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 index f45cc882d..e85bab117 100644 --- 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 & 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 & 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 & 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 index 14f517695..fcd63a46e 100644 --- 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 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) { diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp index 419b22099..d2a845f58 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp @@ -42,7 +42,9 @@ class AttackerPlanner : public PlannerBase crane::RobotCommandWrapper target(robot_id.robot_id, world_model); auto robot = world_model->getRobot(robot_id); - 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) { @@ -97,34 +99,6 @@ class AttackerPlanner : public PlannerBase return 100.0 / std::max(world_model->getSquareDistanceFromRobotToBall(robot->id), 0.01); }); } - - auto getBestShootTargetWithWidth() -> std::pair - { - 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 diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp index 46e30b5a5..15c116f51 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp @@ -99,7 +99,8 @@ class ReceivePlanner : public PlannerBase bg::closest_point(robot_info->pose.pos, ball_line, result); // ゴールとボールの中間方向を向く - 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); @@ -130,7 +131,8 @@ class ReceivePlanner : public PlannerBase continue; } - double score = getLargestGoalAngleWidthFromPosition(dpps_point); + auto [angle, width] = world_model->getLargestGoalAngleRangeFromPoint(dpps_point); + double score = width; const double dist = world_model->getDistanceFromRobot(robot, dpps_point); score = score * (1.0 - dist / 10.0); @@ -145,7 +147,7 @@ class ReceivePlanner : public PlannerBase // ゴールとボールの中間方向を向く Point target_pos{target.latest_msg.target_x.front(), target.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(); target.setTargetTheta(getAngle(to_goal + to_ball)); @@ -250,50 +252,6 @@ class ReceivePlanner : public PlannerBase return ret; } - 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> getPositionsWithScore(Segment ball_line, Point next_target) { auto points = getPoints(ball_line, 0.05); diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp index 2b8a1e06f..90aed1824 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -476,6 +477,54 @@ struct WorldModelWrapper [[nodiscard]] uint8_t getTheirGoalieId() const { return latest_msg.their_goalie_id; } + /** + * + * @param from + * @return {angle, width} + */ + auto getLargestGoalAngleRangeFromPoint(Point from) -> std::pair + { + Interval goal_range; + + auto goal_posts = getTheirGoalPosts(); + if (goal_posts.first.x() < 0.) { + goal_range.append( + normalizeAngle(getAngle(goal_posts.first - from) + M_PI), + normalizeAngle(getAngle(goal_posts.second - from) + M_PI)); + } else { + goal_range.append(getAngle(goal_posts.first - from), getAngle(goal_posts.second - from)); + } + + for (auto & enemy : theirs.getAvailableRobots()) { + double distance = enemy->getDistance(from); + constexpr double MACHINE_RADIUS = 0.1; + + double center_angle = [&]() { + if (goal_posts.first.x() < 0.) { + return normalizeAngle(getAngle(enemy->pose.pos - from) + M_PI); + } else { + return getAngle(enemy->pose.pos - from); + } + }(); + 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 = [&]() { + if (goal_posts.first.x() < 0.) { + return normalizeAngle((largest_interval.first + largest_interval.second) / 2.0 - M_PI); + } else { + return (largest_interval.first + largest_interval.second) / 2.0; + } + }(); + + return {target_angle, largest_interval.second - largest_interval.first}; + } + TeamInfo ours; TeamInfo theirs;