diff --git a/crane_bringup/launch/crane.launch.xml b/crane_bringup/launch/crane.launch.xml index fc5998ddf..2767e6fe8 100644 --- a/crane_bringup/launch/crane.launch.xml +++ b/crane_bringup/launch/crane.launch.xml @@ -56,12 +56,14 @@ - + - + + + @@ -74,6 +76,8 @@ + + diff --git a/crane_robot_skills/include/crane_robot_skills/attacker.hpp b/crane_robot_skills/include/crane_robot_skills/attacker.hpp index 471c36847..91bb10af9 100644 --- a/crane_robot_skills/include/crane_robot_skills/attacker.hpp +++ b/crane_robot_skills/include/crane_robot_skills/attacker.hpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -23,6 +24,8 @@ namespace crane::skills enum class AttackerState { ENTRY_POINT, FORCED_PASS, + CUT_THEIR_PASS, + STEAL_BALL, REDIRECT_GOAL_KICK, GOAL_KICK, CLEARING_KICK, @@ -55,6 +58,8 @@ class Attacker : public SkillBaseWithState return 0.5; case crane_msgs::msg::PlaySituation::STOP: return 0.5; + case crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT: + return 0.5; default: return 0.0; } diff --git a/crane_robot_skills/include/crane_robot_skills/steal_ball.hpp b/crane_robot_skills/include/crane_robot_skills/steal_ball.hpp index c531a5f98..bf81ec0fc 100644 --- a/crane_robot_skills/include/crane_robot_skills/steal_ball.hpp +++ b/crane_robot_skills/include/crane_robot_skills/steal_ball.hpp @@ -19,8 +19,6 @@ namespace crane::skills enum class StealBallState { MOVE_TO_FRONT, STEAL, - PASS, - INTERCEPT, }; class StealBall : public SkillBaseWithState { diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 9771a8cea..74e3f70fd 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -17,7 +17,8 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) kick_skill(base), goal_kick_skill(base), receive_skill(base), - redirect_skill(base) + redirect_skill(base), + steal_ball_skill(base) { setParameter("receiver_id", 0); addStateFunction( @@ -83,7 +84,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) Point target = world_model()->ball.pos + getVerticalVec(receiver->pose.pos - world_model()->ball.pos).normalized() * 0.3; - command.setTargetPosition(target).lookAtBallFrom(target); + command.setTargetPosition(target).lookAtBallFrom(target).enableBallAvoidance(); if (robot()->getDistance(target) < 0.1) { forced_pass_phase = 1; } @@ -111,6 +112,40 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) return Status::RUNNING; }); + addTransition(AttackerState::ENTRY_POINT, AttackerState::CUT_THEIR_PASS, [this]() -> bool { + return not world_model()->isOurBallByBallOwnerCalculator() && world_model()->ball.isMoving(0.2); + }); + + addTransition(AttackerState::CUT_THEIR_PASS, AttackerState::ENTRY_POINT, [this]() -> bool { + return world_model()->isOurBallByBallOwnerCalculator(); + }); + + addStateFunction( + AttackerState::CUT_THEIR_PASS, + [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + return receive_skill.run(visualizer); + }); + + addTransition(AttackerState::ENTRY_POINT, AttackerState::STEAL_BALL, [this]() -> bool { + // 止まっているボールを相手が持っているとき + return not world_model()->isOurBallByBallOwnerCalculator() && + world_model()->ball.isStopped(0.1) && + world_model() + ->getNearestRobotWithDistanceFromPoint( + world_model()->ball.pos, world_model()->theirs.getAvailableRobots()) + .second < 0.5; + }); + + addTransition(AttackerState::STEAL_BALL, AttackerState::ENTRY_POINT, [this]() -> bool { + return world_model()->isOurBallByBallOwnerCalculator(); + }); + + addStateFunction( + AttackerState::STEAL_BALL, + [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + return steal_ball_skill.run(visualizer); + }); + addTransition(AttackerState::ENTRY_POINT, AttackerState::REDIRECT_GOAL_KICK, [this]() -> bool { // ボールが遠くにいる if ( @@ -129,6 +164,9 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) // ボールが止まっている if (world_model()->ball.vel.norm() < 0.5) { return true; + } else if (not world_model()->isOurBallByBallOwnerCalculator()) { + // 敵にボールを奪われた + return true; } else { return false; } @@ -163,25 +201,33 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) addTransition(AttackerState::ENTRY_POINT, AttackerState::GOAL_KICK, [this]() -> bool { auto [best_angle, goal_angle_width] = world_model()->getLargestGoalAngleRangeFromPoint(world_model()->ball.pos); + // ボールが近い条件はいらないかも? return robot()->getDistance(world_model()->ball.pos) < 2.0 && goal_angle_width * 180.0 / M_PI > 5.; }); + addTransition(AttackerState::GOAL_KICK, AttackerState::ENTRY_POINT, [this]() -> bool { + // 敵にボールを奪われた + return not world_model()->isOurBallByBallOwnerCalculator(); + }); + addStateFunction( AttackerState::GOAL_KICK, [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { return goal_kick_skill.run(visualizer); }); - addTransition( - AttackerState::ENTRY_POINT, AttackerState::CLEARING_KICK, [this]() -> bool { return false; }); + addTransition(AttackerState::ENTRY_POINT, AttackerState::CLEARING_KICK, [this]() -> bool { + // 未実装:やばいときに蹴る + return false; + }); addStateFunction( AttackerState::CLEARING_KICK, [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); kick_skill.setParameter("kick_power", 1.0); - kick_skill.setParameter("dot_threshold", 0.95); + kick_skill.setParameter("dot_threshold", 0.9); kick_skill.setParameter("kick_with_chip", true); return kick_skill.run(visualizer); }); @@ -227,6 +273,11 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) return best_score > 0.5; }); + addTransition(AttackerState::STANDARD_PASS, AttackerState::ENTRY_POINT, [this]() -> bool { + // 敵にボールを奪われた + return not world_model()->isOurBallByBallOwnerCalculator(); + }); + addStateFunction( AttackerState::STANDARD_PASS, [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { @@ -290,6 +341,11 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) goal_angle_width * 180.0 / M_PI > 1.; }); + addTransition(AttackerState::LOW_CHANCE_GOAL_KICK, AttackerState::ENTRY_POINT, [this]() -> bool { + // 敵にボールを奪われた + return not world_model()->isOurBallByBallOwnerCalculator(); + }); + addStateFunction( AttackerState::LOW_CHANCE_GOAL_KICK, [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { @@ -305,6 +361,12 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) x_diff_with_their_goal >= world_model()->field_size.x() * 0.5; }); + addTransition( + AttackerState::MOVE_BALL_TO_OPPONENT_HALF, AttackerState::ENTRY_POINT, [this]() -> bool { + // 敵にボールを奪われた + return not world_model()->isOurBallByBallOwnerCalculator(); + }); + addStateFunction( AttackerState::MOVE_BALL_TO_OPPONENT_HALF, [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { @@ -325,6 +387,9 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) // ボールが止まっている if (world_model()->ball.vel.norm() < 0.5) { return true; + } else if (not world_model()->isOurBallByBallOwnerCalculator()) { + // 敵にボールを奪われた + return true; } else { return false; } diff --git a/crane_robot_skills/src/steal_ball.cpp b/crane_robot_skills/src/steal_ball.cpp index c0e2a1afc..d6211677f 100644 --- a/crane_robot_skills/src/steal_ball.cpp +++ b/crane_robot_skills/src/steal_ball.cpp @@ -44,19 +44,6 @@ StealBall::StealBall(RobotCommandWrapperBase::SharedPtr & base) return skill_state == Status::SUCCESS; }); - // 敵よりもボールに近い場合はパス - addTransition(StealBallState::MOVE_TO_FRONT, StealBallState::PASS, [this]() { - auto theirs = world_model()->theirs.getAvailableRobots(); - if (not theirs.empty()) { - auto [their_attacker, their_distance] = - world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs); - double our_distance = robot()->getDistance(world_model()->ball.pos); - return our_distance < their_distance - 0.2; - } else { - return true; - } - }); - addStateFunction( StealBallState::STEAL, [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { @@ -68,7 +55,7 @@ StealBall::StealBall(RobotCommandWrapperBase::SharedPtr & base) world_model()->ball.pos, getAngle(world_model()->ball.pos - robot()->pose.pos)); command.dribble(0.5); } else if (method == "side") { - if (robot()->getDistance(world_model()->ball.pos) < (0.085 + 0.000)) { + if (robot()->getDistance(world_model()->ball.pos) < (0.085 - 0.030)) { // ロボット半径より近くに来れば急回転して刈り取れる command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos) + M_PI / 2); } else { @@ -79,111 +66,5 @@ StealBall::StealBall(RobotCommandWrapperBase::SharedPtr & base) } return Status::RUNNING; }); - - // 敵よりもボールに近い場合はパス - addTransition(StealBallState::STEAL, StealBallState::PASS, [this]() { - auto theirs = world_model()->theirs.getAvailableRobots(); - if (not theirs.empty()) { - auto [their_attacker, their_distance] = - world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs); - double our_distance = robot()->getDistance(world_model()->ball.pos); - return our_distance < their_distance - 0.2; - } else { - return true; - } - }); - - addStateFunction( - StealBallState::PASS, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { - if (attacker_skill == nullptr) { - attacker_skill = std::make_shared(command_base); - } - auto ours = world_model()->ours.getAvailableRobots(robot()->id); - ours.erase( - std::remove_if( - ours.begin(), ours.end(), - [this](auto e) { - return e->getDistance(world_model()->getTheirGoalCenter()) > - robot()->getDistance(world_model()->getTheirGoalCenter()); - }), - ours.end()); - if (not ours.empty()) { - auto [target_bot, distance] = world_model()->getNearestRobotWithDistanceFromPoint( - world_model()->getTheirGoalCenter(), ours); - attacker_skill->setParameter("receiver_id", target_bot->id); - } - return attacker_skill->run(visualizer); - }); - - addTransition(StealBallState::PASS, StealBallState::MOVE_TO_FRONT, [this]() { - auto theirs = world_model()->theirs.getAvailableRobots(); - if (theirs.empty()) { - return false; - } else { - auto [their_attacker, their_distance] = - world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs); - double our_distance = robot()->getDistance(world_model()->ball.pos); - return our_distance > their_distance; - } - }); - - addTransition(StealBallState::PASS, StealBallState::INTERCEPT, [this]() { - return world_model()->ball.vel.norm() > 0.5; - }); - - addTransition(StealBallState::STEAL, StealBallState::INTERCEPT, [this]() { - return world_model()->ball.vel.norm() > 0.5; - }); - - addTransition(StealBallState::MOVE_TO_FRONT, StealBallState::INTERCEPT, [this]() { - return world_model()->ball.vel.norm() > 0.5; - }); - - addStateFunction(StealBallState::INTERCEPT, [this](const ConsaiVisualizerWrapper::SharedPtr &) { - Point vel_seg = world_model()->ball.vel * 5.0; - if (vel_seg.norm() < 1.0) { - vel_seg = vel_seg.normalized() * 1.0; - } - Segment ball_line{world_model()->ball.pos, world_model()->ball.pos + vel_seg}; - - Point across_point = [&]() { - const double OFFSET = 0.3; - const double X = world_model()->field_size.x() / 2.0 - OFFSET; - const double Y = world_model()->field_size.y() / 2.0 - OFFSET; - - Segment seg1{Point(X, Y), Point(X, -Y)}; - Segment seg2{Point(-X, Y), Point(-X, -Y)}; - Segment seg3{Point(Y, X), Point(-Y, X)}; - Segment seg4{Point(Y, -X), Point(-Y, -X)}; - std::vector intersections; - if (bg::intersection(ball_line, seg1, intersections); not intersections.empty()) { - return intersections.front(); - } else if (bg::intersection(ball_line, seg2, intersections); not intersections.empty()) { - return intersections.front(); - } else if (bg::intersection(ball_line, seg3, intersections); not intersections.empty()) { - return intersections.front(); - } else if (bg::intersection(ball_line, seg4, intersections); not intersections.empty()) { - return intersections.front(); - } else { - return ball_line.second; - } - }(); - - // ゴールとボールの中間方向を向く - auto [goal_angle, width] = world_model()->getLargestGoalAngleRangeFromPoint(across_point); - auto to_goal = getNormVec(goal_angle); - auto to_ball = (world_model()->ball.pos - across_point).normalized(); - double intermediate_angle = getAngle(2 * to_goal + to_ball); - command.liftUpDribbler(); - command.kickStraight(getParameter("kicker_power")); - command.setDribblerTargetPosition(across_point, intermediate_angle); - - return Status::RUNNING; - }); - - addTransition(StealBallState::INTERCEPT, StealBallState::MOVE_TO_FRONT, [this]() { - return world_model()->ball.vel.norm() < 0.3; - }); } } // namespace crane::skills diff --git a/crane_sender/include/crane_sender/sender_base.hpp b/crane_sender/include/crane_sender/sender_base.hpp index 4e194d656..56c541896 100644 --- a/crane_sender/include/crane_sender/sender_base.hpp +++ b/crane_sender/include/crane_sender/sender_base.hpp @@ -29,6 +29,12 @@ class SenderBase : public rclcpp::Node declare_parameter("delay_s", 0.0); get_parameter("delay_s", delay_s); + declare_parameter("kick_power_limit_straight", 1.0); + get_parameter("kick_power_limit_straight", kick_power_limit_straight); + + declare_parameter("kick_power_limit_chip", 1.0); + get_parameter("kick_power_limit_chip", kick_power_limit_chip); + declare_parameter("latency_ms", 0.0); get_parameter("latency_ms", current_latency_ms); @@ -57,6 +63,10 @@ class SenderBase : public rclcpp::Node double current_latency_ms = 0.0; + double kick_power_limit_straight; + + double kick_power_limit_chip; + virtual void sendCommands(const crane_msgs::msg::RobotCommands & msg) = 0; float normalizeAngle(float angle_rad) const diff --git a/crane_sender/include/crane_sender/sim_sender.hpp b/crane_sender/include/crane_sender/sim_sender.hpp index 8e43e1f3c..f354d291d 100644 --- a/crane_sender/include/crane_sender/sim_sender.hpp +++ b/crane_sender/include/crane_sender/sim_sender.hpp @@ -217,7 +217,13 @@ class SimSenderComponent : public SenderBase } // キック速度 - double kick_speed = command.kick_power * MAX_KICK_SPEED; + double kick_speed = MAX_KICK_SPEED * [&]() { + if (command.chip_enable) { + return std::min(command.kick_power, static_cast(kick_power_limit_chip)); + } else { + return std::min(command.kick_power, static_cast(kick_power_limit_straight)); + } + }(); // チップキック if (command.chip_enable) { diff --git a/crane_sender/src/ibis_sender_node.cpp b/crane_sender/src/ibis_sender_node.cpp index 32f5cca31..da27e31bb 100644 --- a/crane_sender/src/ibis_sender_node.cpp +++ b/crane_sender/src/ibis_sender_node.cpp @@ -159,7 +159,13 @@ class IbisSenderNode : public SenderBase }(); packet.latency_time_ms = current_latency_ms; // TODO(Hans): ちゃんと計測する packet.target_global_theta = command.target_theta; - packet.kick_power = std::clamp(command.kick_power, 0.f, 1.f); + packet.kick_power = [&]() { + if (command.chip_enable) { + return std::clamp(command.kick_power, 0.f, static_cast(kick_power_limit_chip)); + } else { + return std::clamp(command.kick_power, 0.f, static_cast(kick_power_limit_straight)); + } + }(); packet.dribble_power = std::clamp(command.dribble_power, 0.f, 1.f); packet.enable_chip = command.chip_enable; packet.lift_dribbler = command.lift_up_dribbler_flag; diff --git a/docker/docker-compose.yaml b/docker/docker-compose.yaml index 6c77085d9..654d201e3 100644 --- a/docker/docker-compose.yaml +++ b/docker/docker-compose.yaml @@ -53,18 +53,18 @@ services: ports: - 8084:8084/tcp -# autoref-tigers: -# image: tigersmannheim/auto-referee:1.2.0 -# command: -# - -a # active mode (connect to GC) -# - -hl # headless -# - --visionAddress -# - 224.5.23.2:10006 -# - --refereeAddress -# - 224.5.23.1:11003 -# - --trackerAddress -# - 224.5.23.2:11010 -# network_mode: host + autoref-tigers: + image: tigersmannheim/auto-referee:1.2.0 + command: + - -a # active mode (connect to GC) + - -hl # headless + - --visionAddress + - 224.5.23.2:10006 + - --refereeAddress + - 224.5.23.1:11111 + - --trackerAddress + - 224.5.23.2:11010 + network_mode: host # autoref-erforce: # image: roboticserlangen/autoref:commit-6f15f574ea80 diff --git a/docs/attacker.md b/docs/attacker.md new file mode 100644 index 000000000..8323f7d18 --- /dev/null +++ b/docs/attacker.md @@ -0,0 +1,58 @@ +# Attackerスキル + +- ENTRY_POINT +- FORCED_PASS +- CUT_THEIR_PASS +- STEAL_BALL +- REDIRECT_GOAL_KICK +- GOAL_KICK +- CLEARING_KICK +- STANDARD_PASS +- LOW_CHANCE_GOAL_KICK +- MOVE_BALL_TO_OPPONENT_HALF +- RECEIVE_BALL + +```mermaid +graph TD + ENTRY_POINT(ENTRY_POINT) -.-> FORCED_PASS_C{味方セットプレー?} +subgraph セットプレー + FORCED_PASS_C -- YES --> FORCED_PASS +end + +%%subgraph 敵ボール対処 +%% STEAL_BALL_C +%%end + +subgraph シュートチャンスは逃さない + +end + +subgraph 敵ボール対処 + FORCED_PASS_C -- NO --> CUT_THEIR_PASS_C{動いている敵ボール?} + CUT_THEIR_PASS_C -- NO --> STEAL_BALL_C{止まっている敵ボール & ボールに敵が近い?} + CUT_THEIR_PASS_C -- YES --> CUT_THEIR_PASS +end + +subgraph シュートチャンスは逃さない + STEAL_BALL_C -- NO --> REDIRECT_GOAL_KICK_C{ボールが遠い&ゴールが見えている} + REDIRECT_GOAL_KICK_C -- NO --> GOAL_KICK_C{ボールが近い&ゴールが見えている} + REDIRECT_GOAL_KICK_C -- YES --> REDIRECT_GOAL_KICK + GOAL_KICK_C -- YES --> GOAL_KICK +end + +subgraph 敵ボール対処 + STEAL_BALL_C -- YES --> STEAL_BALL +end + +subgraph 状況改善 + GOAL_KICK_C -- NO --> CLEARING_KICK_C{やばい状況?(未実装)} + CLEARING_KICK_C -- NO --> STANDARD_PASS_C{パスできそうな相手がいる?} + CLEARING_KICK_C -- YES --> CLEARING_KICK + STANDARD_PASS_C -- YES --> STANDARD_PASS + STANDARD_PASS_C -- NO --> LOW_CHANCE_GOAL_KICK_C{低確率でもシュートできそう(相手コート限定)?} + LOW_CHANCE_GOAL_KICK_C -- NO --> MOVE_BALL_TO_OPPONENT_HALF_C{自コートでボールを持っている?} + LOW_CHANCE_GOAL_KICK_C -- YES --> LOW_CHANCE_GOAL_KICK + MOVE_BALL_TO_OPPONENT_HALF_C -- YES --> MOVE_BALL_TO_OPPONENT_HALF + MOVE_BALL_TO_OPPONENT_HALF_C -- NO --> RECEIVE_BALL +end +``` diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp index b639f69e0..157ded3de 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp @@ -106,7 +106,7 @@ class RobotCommandWrapperCommon return static_cast(*this); } - T & stopHere() + virtual T & stopHere() { command->latest_msg.stop_flag = true; return static_cast(*this); @@ -318,6 +318,12 @@ class RobotCommandWrapperPosition : public RobotCommandWrapperCommonlatest_msg.stop_flag = true; + return setTargetPosition(command->robot->pose.pos); + } }; class RobotCommandWrapperSimpleVelocity 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 6767b7187..3f54a3b4e 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 @@ -330,6 +330,11 @@ struct WorldModelWrapper return ball_owner_calculator.getTheirFrontier(); } + [[nodiscard]] auto isOurBallByBallOwnerCalculator() const + { + return ball_owner_calculator.isOurBall(); + } + class PointChecker { public: