diff --git a/.github/workflows/scenario_test.yaml b/.github/workflows/scenario_test.yaml index 59577ad2c..0271361b8 100644 --- a/.github/workflows/scenario_test.yaml +++ b/.github/workflows/scenario_test.yaml @@ -91,6 +91,7 @@ jobs: env: - TEST: STOP_AVOID_BALL - TEST: STOP_ROBOT_SPEED + - TEST: emit_from_penalty_01 runs-on: ubuntu-latest steps: diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index 2c8830b87..4258ae3d8 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -50,15 +50,17 @@ void Goalie::emitBallFromPenaltyArea( 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; - } + if ( + std::abs(r->pose.pos.x() - world_model->getOurGoalCenter().x()) < + world_model->getDefenseHeight()) { + // ゴールラインに近いロボットは除外 + return true; + } else if (world_model->getDistanceFromRobotToBall(r->getID()) < 0.5) { + // ボールに近いロボットは除外 + return true; + } else { + return false; } - return false; }), passable_robot_list.end()); @@ -80,19 +82,17 @@ void Goalie::emitBallFromPenaltyArea( .dot((pass_target - world_model->ball.pos).normalized()); // ボールと目標の延長線上にいない && 角度があってないときは，中間ポイントを経由 if (dot < 0.9 || std::abs(getAngleDiff(angle_ball_to_target, command->robot->pose.theta)) > 0.1) { - command->setTargetPosition(intermediate_point); - command->enableCollisionAvoidance(); + command->setTargetPosition(intermediate_point).enableCollisionAvoidance().enableBallAvoidance(); } else { - command->setTargetPosition(world_model->ball.pos); - command->kickWithChip(1.0).disableCollisionAvoidance(); - // command->liftUpDribbler(); - // command->kickStraight(0.2).disableCollisionAvoidance(); - command->enableCollisionAvoidance(); - command->disableBallAvoidance(); + command->setTargetPosition(world_model->ball.pos) + .kickWithChip(1.0) + .disableCollisionAvoidance() + .enableCollisionAvoidance() + .disableBallAvoidance(); } - command->setTargetTheta(getAngle(pass_target - command->robot->pose.pos)); - command->disableGoalAreaAvoidance(); - command->disableRuleAreaAvoidance(); + command->setTargetTheta(getAngle(pass_target - command->robot->pose.pos)) + .disableGoalAreaAvoidance() + .disableRuleAreaAvoidance(); } void Goalie::inplay( @@ -105,22 +105,28 @@ void Goalie::inplay( Segment goal_line(goals.first, goals.second); Segment ball_line(ball.pos, ball.pos + ball.vel.normalized() * 20.f); auto intersections = getIntersections(ball_line, Segment{goals.first, goals.second}); - command->setTerminalVelocity(0.0); - command->disableGoalAreaAvoidance(); - command->disableBallAvoidance(); - command->disableRuleAreaAvoidance(); + command->setTerminalVelocity(0.0) + .disableGoalAreaAvoidance() + .disableBallAvoidance() + .disableRuleAreaAvoidance(); if (not intersections.empty() && world_model->ball.vel.norm() > 0.3f) { // シュートブロック phase = "シュートブロック"; auto result = getClosestPointAndDistance(ball_line, command->robot->pose.pos); - command->setTargetPosition(result.closest_point); - command->lookAtBallFrom(result.closest_point); - if (command->robot->getDistance(result.closest_point) > 0.05) { + auto target = [&]() { + if (not world_model->isFieldInside(result.closest_point)) { + // フィールド外（=ゴール内）でのセーブは避ける + return intersections.front(); + } else { + return result.closest_point; + } + }(); + + command->setTargetPosition(target).lookAtBallFrom(target); + if (command->robot->getDistance(target) > 0.05) { // なりふり構わず爆加速 - command->setTerminalVelocity(2.0); - command->setMaxAcceleration(5.0); - command->setMaxVelocity(5.0); + command->setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); } } else { if ( @@ -158,8 +164,7 @@ void Goalie::inplay( if (not world_model->isFieldInside(ball.pos)) { phase += "(範囲外なので正面に構える)"; - command->setTargetPosition(goal_center); - command->lookAt(Point(0, 0)); + command->setTargetPosition(goal_center).lookAt(Point(0, 0)); } else { Point threat_point; if (distance < 2.0) { @@ -186,13 +191,10 @@ void Goalie::inplay( Point wait_point = weak_point + (threat_point - weak_point).normalized() * BLOCK_DIST; - command->setTargetPosition(wait_point); - command->lookAtBallFrom(wait_point); + command->setTargetPosition(wait_point).lookAtBallFrom(wait_point); if (command->robot->getDistance(wait_point) > 0.05) { // なりふり構わず爆加速 - command->setTerminalVelocity(2.0); - command->setMaxAcceleration(5.0); - command->setMaxVelocity(5.0); + command->setTerminalVelocity(2.0).setMaxAcceleration(5.0).setMaxVelocity(5.0); } } } diff --git a/docker/scenario/docker-compose.yaml b/docker/scenario/docker-compose.yaml index f9e85acbe..959d16c60 100644 --- a/docker/scenario/docker-compose.yaml +++ b/docker/scenario/docker-compose.yaml @@ -1,7 +1,7 @@ version: "3.1" services: grsim: - image: ghcr.io/ibis-ssl/grsim:customized + image: ghcr.io/ssl-roots/docker_images/grsim:main container_name: grsim network_mode: host command: | diff --git a/scenario_test/emit_from_penalty_01.py b/scenario_test/emit_from_penalty_01.py new file mode 100644 index 000000000..ff5a79bd5 --- /dev/null +++ b/scenario_test/emit_from_penalty_01.py @@ -0,0 +1,24 @@ +import math +import time +from rcst.communication import Communication +from rcst import calc +from rcst.ball import Ball +from rcst.robot import RobotDict + + +def is_in_penalty_area(x: float, y: float) -> bool: + return math.fabs(x) >= 6.0 and math.fabs(y) <= 1.8 + + +def test_emit_from_penalty_01(rcst_comm: Communication): + rcst_comm.send_empty_world() + rcst_comm.send_ball(5.0, 1.0) + rcst_comm.send_yellow_robot(0, 6.0, 0, 0) + rcst_comm.change_referee_command("FORCE_START", 3.0) + + rcst_comm.observer.reset() + time.sleep(5) + success = not is_in_penalty_area( + rcst_comm.observer.get_world().get_ball().x, rcst_comm.observer.get_world().get_ball().y + ) + assert success is True