diff --git a/docs/fsm-diagrams.md b/docs/fsm-diagrams.md index 8059b8e10d..742df21c2a 100644 --- a/docs/fsm-diagrams.md +++ b/docs/fsm-diagrams.md @@ -469,6 +469,7 @@ stateDiagram-v2 classDef terminate fill:white,color:black,font-weight:bold direction LR [*] --> WaitingForPassState +WaitingForPassState --> WaitingForPassState : [shouldMoveAwayFromShot]\nmoveAwayFromShot WaitingForPassState --> WaitingForPassState : [!passStarted]\nupdateReceive WaitingForPassState --> OneTouchShotState : [passStarted && onetouchPossible]\nupdateOnetouch WaitingForPassState --> ReceiveAndDribbleState : [passStarted && !onetouchPossible]\nupdateReceive diff --git a/src/proto/tactic.proto b/src/proto/tactic.proto index afc537c1b4..45b9d8c35b 100644 --- a/src/proto/tactic.proto +++ b/src/proto/tactic.proto @@ -160,7 +160,7 @@ message PivotKickTactic message ReceiverTactic { // The pass to receive - optional Pass pass = 1; + optional Pass pass_ = 1; // needs the _ because pass is a keyword in python // If set to true, we will only receive and dribble required bool disable_one_touch_shot = 2; } diff --git a/src/software/ai/hl/stp/tactic/receiver/BUILD b/src/software/ai/hl/stp/tactic/receiver/BUILD index 8548bfe80e..9ff8ee7728 100644 --- a/src/software/ai/hl/stp/tactic/receiver/BUILD +++ b/src/software/ai/hl/stp/tactic/receiver/BUILD @@ -1,3 +1,5 @@ +load("@simulated_tests_deps//:requirements.bzl", "requirement") + package(default_visibility = ["//visibility:public"]) cc_library( @@ -34,7 +36,7 @@ cc_test( ) cc_test( - name = "receiver_tactic_test", + name = "receiver_tactic_cpp_test", srcs = ["receiver_tactic_test.cpp"], deps = [ ":receiver_tactic", @@ -47,3 +49,17 @@ cc_test( "//software/world", ], ) + +py_test( + name = "receiver_tactic_test", + srcs = ["receiver_tactic_test.py"], + tags = [ + "exclusive", + ], + deps = [ + "//software:conftest", + "//software/simulated_tests:speed_threshold_helpers", + "//software/simulated_tests:validation", + requirement("pytest"), + ], +) diff --git a/src/software/ai/hl/stp/tactic/receiver/receiver_fsm.cpp b/src/software/ai/hl/stp/tactic/receiver/receiver_fsm.cpp index c1982d5599..7a3660cc0d 100644 --- a/src/software/ai/hl/stp/tactic/receiver/receiver_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/receiver/receiver_fsm.cpp @@ -174,6 +174,26 @@ void ReceiverFSM::adjustReceive(const Update& event) } } +void ReceiverFSM::moveAwayFromShot(const Update& event) +{ + Point ball = event.common.world_ptr->ball().position(); + Point robot = event.common.robot.position(); + + Vector between_robot_and_ball = ball - robot; + Vector side_step_direction = between_robot_and_ball.normalize().perpendicular(); + + Point side_step_position = robot + side_step_direction; + + Angle orientation = (side_step_position - ball).orientation(); + + event.common.set_primitive(std::make_unique( + event.common.robot, side_step_position, orientation, + TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, + TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, + TbotsProto::DribblerMode::MAX_FORCE, TbotsProto::BallCollisionType::ALLOW, + AutoChipOrKick{AutoChipOrKickMode::OFF, {0}})); +} + bool ReceiverFSM::passStarted(const Update& event) { return event.common.world_ptr->ball().hasBallBeenKicked( @@ -207,3 +227,18 @@ bool ReceiverFSM::strayPass(const Update& event) return stray_pass; } + +bool ReceiverFSM::shouldMoveAwayFromShot(const Update& event) +{ + Point ball = event.common.world_ptr->ball().position(); + Point goal = event.common.world_ptr->field().enemyGoalpostPos(); + + Vector ball_expand = (ball - goal).normalize(0.5).perpendicular(); + Vector goal_expand = Vector(0.5, 0.0); + + Point robot = event.common.robot.position(); + + return contains(Polygon{ball - ball_expand, ball + ball_expand, goal + goal_expand, + goal - goal_expand}, + robot); +} diff --git a/src/software/ai/hl/stp/tactic/receiver/receiver_fsm.h b/src/software/ai/hl/stp/tactic/receiver/receiver_fsm.h index 051ea8f7a7..cf2a2513f4 100644 --- a/src/software/ai/hl/stp/tactic/receiver/receiver_fsm.h +++ b/src/software/ai/hl/stp/tactic/receiver/receiver_fsm.h @@ -114,6 +114,14 @@ struct ReceiverFSM */ void adjustReceive(const Update& event); + /** + * Move the robot away from the planned shot path by moving + * perpendicular to the path by a metre. + * + * @param event ReceiverFSM::Update event + */ + void moveAwayFromShot(const Update& event); + /** * Guard that checks if the ball has been kicked * @@ -141,6 +149,14 @@ struct ReceiverFSM */ bool strayPass(const Update& event); + /** + * Checks if attacker wants to shoot and receiver is in the way. + * + * @param event ReceiverFSM::Update event + * @return true if should move away from shot attempt + */ + bool shouldMoveAwayFromShot(const Update& event); + auto operator()() { using namespace boost::sml; @@ -154,14 +170,18 @@ struct ReceiverFSM DEFINE_SML_GUARD(passStarted) DEFINE_SML_GUARD(passFinished) DEFINE_SML_GUARD(strayPass) + DEFINE_SML_GUARD(shouldMoveAwayFromShot) DEFINE_SML_ACTION(updateOnetouch) DEFINE_SML_ACTION(updateReceive) DEFINE_SML_ACTION(adjustReceive) + DEFINE_SML_ACTION(moveAwayFromShot) return make_transition_table( // src_state + event [guard] / action = dest_state - *WaitingForPassState_S + Update_E[!passStarted_G] / updateReceive_A, + *WaitingForPassState_S + + Update_E[shouldMoveAwayFromShot_G] / moveAwayFromShot_A, + WaitingForPassState_S + Update_E[!passStarted_G] / updateReceive_A, WaitingForPassState_S + Update_E[passStarted_G && onetouchPossible_G] / updateOnetouch_A = OneTouchShotState_S, WaitingForPassState_S + Update_E[passStarted_G && !onetouchPossible_G] / diff --git a/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.py b/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.py new file mode 100644 index 0000000000..4a7fee2a68 --- /dev/null +++ b/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.py @@ -0,0 +1,149 @@ +import pytest + +from proto.import_all_protos import * +import software.python_bindings as tbots_cpp +from software.simulated_tests.ball_enters_region import BallEventuallyEntersRegion +from software.simulated_tests.simulated_test_fixture import ( + pytest_main, +) +from proto.message_translation.tbots_protobuf import create_world_state + +corner = tbots_cpp.Field.createSSLDivisionBField().enemyDefenseArea().negXNegYCorner() +centre = tbots_cpp.Field.createSSLDivisionBField().enemyDefenseArea().centre() + + +@pytest.mark.parametrize( + "blue_bots, yellow_bots, ball_initial_pos, ball_initial_velocity", + [ + # Test bottom left medium distance from attacker + ( + [ + corner + tbots_cpp.Vector(-1.5, -1.5), + corner + tbots_cpp.Vector(-0.5, -0.5), + ], + [], + corner + tbots_cpp.Vector(-1.3, -1.3), + tbots_cpp.Vector(0, 0), + ), + # Test bottom left close distance from attacker + ( + [ + corner + tbots_cpp.Vector(-1.5, -1.5), + corner + tbots_cpp.Vector(-1.0, -1.0), + ], + [], + corner + tbots_cpp.Vector(-1.3, -1.3), + tbots_cpp.Vector(0, 0), + ), + # Test centre medium distance from attacker + ( + [ + centre + tbots_cpp.Vector(-2.5, 0.0), + centre + tbots_cpp.Vector(-1.0, 0.0), + ], + [], + centre + tbots_cpp.Vector(-2.2, 0.0), + tbots_cpp.Vector(0, 0), + ), + # Test centre close distance from attacker + ( + [ + centre + tbots_cpp.Vector(-1.5, 0.0), + centre + tbots_cpp.Vector(-1.0, 0.0), + ], + [], + centre + tbots_cpp.Vector(-1.3, 0.0), + tbots_cpp.Vector(0, 0), + ), + ], +) +def test_receiver_move_away_from_shot_in_progress( + blue_bots, + yellow_bots, + ball_initial_pos, + ball_initial_velocity, + simulated_test_runner, +): + # Setup Robot + def setup(*args): + simulated_test_runner.simulator_proto_unix_io.send_proto( + WorldState, + create_world_state( + blue_robot_locations=blue_bots, + yellow_robot_locations=yellow_bots, + ball_location=ball_initial_pos, + ball_velocity=ball_initial_velocity, + ), + ) + + # Setup Tactic + params = AssignedTacticPlayControlParams() + + passer_point = blue_bots[0] + receiver_point = blue_bots[1] + receive_speed_m_per_s = 2.0 + min_pass_speed_m_per_s = 1.0 + max_pass_speed_m_per_s = 4.0 + pass_to_test = tbots_cpp.Pass.fromDestReceiveSpeed( + passer_point, + receiver_point, + receive_speed_m_per_s, + min_pass_speed_m_per_s, + max_pass_speed_m_per_s, + ) + + possible_pass = Pass( + passer_point=Point( + x_meters=pass_to_test.passerPoint().x(), + y_meters=pass_to_test.passerPoint().y(), + ), + receiver_point=Point( + x_meters=pass_to_test.receiverPoint().x(), + y_meters=pass_to_test.receiverPoint().y(), + ), + pass_speed_m_per_s=pass_to_test.speed(), + ) + + params.assigned_tactics[0].attacker.CopyFrom( + AttackerTactic( + best_pass_so_far=possible_pass, # optional + pass_committed=False, + # chip_target={"x_meters": 0.0, "y_meters": 0.0}, # optional + ) + ) + + params.assigned_tactics[1].receiver.CopyFrom( + ReceiverTactic( + pass_=possible_pass, # optional + disable_one_touch_shot=True, + ) + ) + + simulated_test_runner.blue_full_system_proto_unix_io.send_proto( + AssignedTacticPlayControlParams, params + ) + + # Always Validation + always_validation_sequence_set = [[]] + + # Eventually Validation + eventually_validation_sequence_set = [ + [ + BallEventuallyEntersRegion( + regions=[tbots_cpp.Field.createSSLDivisionBField().enemyGoal()] + ) + ] + ] + + simulated_test_runner.run_test( + setup=setup, + inv_eventually_validation_sequence_set=eventually_validation_sequence_set, + inv_always_validation_sequence_set=always_validation_sequence_set, + ag_eventually_validation_sequence_set=eventually_validation_sequence_set, + ag_always_validation_sequence_set=always_validation_sequence_set, + test_timeout_s=4, + ) + + +if __name__ == "__main__": + pytest_main(__file__) diff --git a/src/software/ai/hl/stp/tactic/tactic_factory.cpp b/src/software/ai/hl/stp/tactic/tactic_factory.cpp index 73f51d2a66..11124b37cc 100644 --- a/src/software/ai/hl/stp/tactic/tactic_factory.cpp +++ b/src/software/ai/hl/stp/tactic/tactic_factory.cpp @@ -179,9 +179,9 @@ std::shared_ptr createTactic(const TbotsProto::ReceiverTactic &tactic_pr { auto tactic = std::make_shared(ai_config.receiver_tactic_config()); std::optional pass = std::nullopt; - if (tactic_proto.has_pass()) + if (tactic_proto.has_pass_()) { - pass = createPass(tactic_proto.pass()); + pass = createPass(tactic_proto.pass_()); } tactic->updateControlParams(pass, tactic_proto.disable_one_touch_shot());