diff --git a/docs/fsm-diagrams.md b/docs/fsm-diagrams.md index 7112661531..4de14de8c3 100644 --- a/docs/fsm-diagrams.md +++ b/docs/fsm-diagrams.md @@ -71,7 +71,10 @@ stateDiagram-v2 classDef terminate fill:white,color:black,font-weight:bold direction LR [*] --> DefenseState -DefenseState --> DefenseState : defendAgainstThreats +DefenseState --> AggressiveDefenseState : [shouldDefendAggressively]\nshadowAndBlockShots +DefenseState --> DefenseState : blockShots +AggressiveDefenseState --> DefenseState : [!shouldDefendAggressively]\nblockShots +AggressiveDefenseState --> AggressiveDefenseState : shadowAndBlockShots Terminate:::terminate --> Terminate:::terminate ``` @@ -457,12 +460,16 @@ classDef terminate fill:white,color:black,font-weight:bold direction LR [*] --> MoveFSM MoveFSM --> BlockPassState : [!enemyThreatHasBall]\nblockPass +MoveFSM --> GoAndStealState : [blockedShot]\ngoAndSteal MoveFSM --> MoveFSM : blockShot -MoveFSM --> StealAndChipState +MoveFSM --> GoAndStealState BlockPassState --> BlockPassState : [!enemyThreatHasBall]\nblockPass BlockPassState --> MoveFSM : [enemyThreatHasBall]\nblockShot -StealAndChipState --> StealAndChipState : [enemyThreatHasBall]\nstealAndChip -StealAndChipState --> Terminate:::terminate : [!enemyThreatHasBall]\nblockPass +GoAndStealState --> GoAndStealState : [enemyThreatHasBall && !contestedBall]\ngoAndSteal +GoAndStealState --> StealAndPullState : [enemyThreatHasBall && contestedBall]\ngoAndSteal +GoAndStealState --> Terminate:::terminate : [!enemyThreatHasBall]\nblockPass +StealAndPullState --> StealAndPullState : [enemyThreatHasBall]\nstealAndPull +StealAndPullState --> Terminate:::terminate : [!enemyThreatHasBall]\nblockPass Terminate:::terminate --> BlockPassState : [!enemyThreatHasBall]\nblockPass Terminate:::terminate --> MoveFSM : [enemyThreatHasBall]\nblockShot Terminate:::terminate --> Terminate:::terminate : SET_STOP_PRIMITIVE_ACTION diff --git a/src/software/ai/evaluation/defender_assignment.h b/src/software/ai/evaluation/defender_assignment.h index 7e7e7c94fb..a2b0c653d4 100644 --- a/src/software/ai/evaluation/defender_assignment.h +++ b/src/software/ai/evaluation/defender_assignment.h @@ -10,7 +10,8 @@ enum DefenderAssignmentType { CREASE_DEFENDER, - PASS_DEFENDER + PASS_DEFENDER, + SHADOW_ENEMY }; // This struct stores the concept of a defender assignment, which describes diff --git a/src/software/ai/hl/stp/play/defense/BUILD b/src/software/ai/hl/stp/play/defense/BUILD index 280127e35f..05cd22e4fc 100644 --- a/src/software/ai/hl/stp/play/defense/BUILD +++ b/src/software/ai/hl/stp/play/defense/BUILD @@ -23,6 +23,7 @@ cc_library( "//software/ai/hl/stp/play/defense:defense_play_base", "//software/ai/hl/stp/tactic/crease_defender:crease_defender_tactic", "//software/ai/hl/stp/tactic/pass_defender:pass_defender_tactic", + "//software/ai/hl/stp/tactic/shadow_enemy:shadow_enemy_tactic", "//software/logger", "//software/util/generic_factory", "@sml", diff --git a/src/software/ai/hl/stp/play/defense/defense_play_base.cpp b/src/software/ai/hl/stp/play/defense/defense_play_base.cpp index 31d951263f..08ab2d8cb4 100644 --- a/src/software/ai/hl/stp/play/defense/defense_play_base.cpp +++ b/src/software/ai/hl/stp/play/defense/defense_play_base.cpp @@ -4,7 +4,7 @@ #include "software/ai/evaluation/enemy_threat.h" DefensePlayFSMBase::DefensePlayFSMBase(TbotsProto::AiConfig ai_config) - : ai_config(ai_config), crease_defenders({}), pass_defenders({}) + : ai_config(ai_config), crease_defenders({}), pass_defenders({}), shadowers({}) { } diff --git a/src/software/ai/hl/stp/play/defense/defense_play_base.h b/src/software/ai/hl/stp/play/defense/defense_play_base.h index fd899b6a96..b7d13c7a43 100644 --- a/src/software/ai/hl/stp/play/defense/defense_play_base.h +++ b/src/software/ai/hl/stp/play/defense/defense_play_base.h @@ -69,4 +69,5 @@ class DefensePlayFSMBase TbotsProto::AiConfig ai_config; std::vector> crease_defenders; std::vector> pass_defenders; + std::vector> shadowers; }; diff --git a/src/software/ai/hl/stp/play/defense/defense_play_fsm.cpp b/src/software/ai/hl/stp/play/defense/defense_play_fsm.cpp index 3495596ed1..f7f5dfe4b8 100644 --- a/src/software/ai/hl/stp/play/defense/defense_play_fsm.cpp +++ b/src/software/ai/hl/stp/play/defense/defense_play_fsm.cpp @@ -2,20 +2,55 @@ #include "software/ai/evaluation/defender_assignment.h" #include "software/ai/evaluation/enemy_threat.h" +#include "software/logger/logger.h" + DefensePlayFSM::DefensePlayFSM(TbotsProto::AiConfig ai_config) : DefensePlayFSMBase::DefensePlayFSMBase(ai_config) { } -void DefensePlayFSM::defendAgainstThreats(const Update& event) +bool DefensePlayFSM::shouldDefendAggressively(const Update& event) +{ + return true; +} + +void DefensePlayFSM::blockShots(const Update& event) +{ + auto enemy_threats = getAllEnemyThreats( + event.common.world_ptr->field(), event.common.world_ptr->friendlyTeam(), + event.common.world_ptr->enemyTeam(), event.common.world_ptr->ball(), false); + + updateCreaseAndPassDefenders(event, enemy_threats); + updateShadowers(event, {}); + + setTactics(event); +} + +void DefensePlayFSM::shadowAndBlockShots(const Update& event) { auto enemy_threats = getAllEnemyThreats( event.common.world_ptr->field(), event.common.world_ptr->friendlyTeam(), event.common.world_ptr->enemyTeam(), event.common.world_ptr->ball(), false); + + updateCreaseAndPassDefenders(event, enemy_threats); + + if (pass_defenders.size() > 0) + { + pass_defenders.erase(pass_defenders.begin()); + updateShadowers(event, {enemy_threats.front()}); + } + + setTactics(event); +} + +void DefensePlayFSM::updateCreaseAndPassDefenders( + const Update& event, const std::vector& enemy_threats) +{ auto assignments = getAllDefenderAssignments( enemy_threats, event.common.world_ptr->field(), event.common.world_ptr->ball(), + ai_config.defense_play_config().defender_assignment_config()); if (assignments.size() == 0) @@ -68,11 +103,43 @@ void DefensePlayFSM::defendAgainstThreats(const Update& event) setAlignment(event, crease_defender_assignments, TbotsProto::BallStealMode::STEAL); updatePassDefenderControlParams(pass_defender_assignments, TbotsProto::BallStealMode::STEAL); +} + +void DefensePlayFSM::updateShadowers(const Update& event, + const std::vector& threats_to_shadow) +{ + setUpShadowers(static_cast(threats_to_shadow.size())); + + for (unsigned int i = 0; i < shadowers.size(); i++) + { + shadowers.at(i)->updateControlParams(threats_to_shadow.at(i), + ROBOT_SHADOWING_DISTANCE_METERS); + } +} + + +void DefensePlayFSM::setUpShadowers(unsigned int num_shadowers) +{ + if (num_shadowers == shadowers.size()) + { + return; + } + + shadowers = std::vector>(num_shadowers); + std::generate(shadowers.begin(), shadowers.end(), + [this]() { return std::make_shared(); }); +} + +void DefensePlayFSM::setTactics(const Update& event) +{ + PriorityTacticVector tactics_to_return = {{}, {}, {}}; - PriorityTacticVector tactics_to_return = {{}, {}}; tactics_to_return[0].insert(tactics_to_return[0].end(), crease_defenders.begin(), crease_defenders.end()); tactics_to_return[1].insert(tactics_to_return[1].end(), pass_defenders.begin(), pass_defenders.end()); + tactics_to_return[2].insert(tactics_to_return[2].end(), shadowers.begin(), + shadowers.end()); + event.common.set_tactics(tactics_to_return); } diff --git a/src/software/ai/hl/stp/play/defense/defense_play_fsm.h b/src/software/ai/hl/stp/play/defense/defense_play_fsm.h index 74356194e3..020874e03c 100644 --- a/src/software/ai/hl/stp/play/defense/defense_play_fsm.h +++ b/src/software/ai/hl/stp/play/defense/defense_play_fsm.h @@ -6,12 +6,13 @@ #include "software/ai/hl/stp/play/play_fsm.h" #include "software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic.h" #include "software/ai/hl/stp/tactic/pass_defender/pass_defender_tactic.h" +#include "software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic.h" #include "software/logger/logger.h" struct DefensePlayFSM : public DefensePlayFSMBase { class DefenseState; - + class AggressiveDefenseState; /** * Creates a defense play FSM * @@ -19,27 +20,94 @@ struct DefensePlayFSM : public DefensePlayFSMBase */ explicit DefensePlayFSM(TbotsProto::AiConfig ai_config); + // Where to shadow + const double ROBOT_SHADOWING_DISTANCE_METERS = ROBOT_MAX_RADIUS_METERS * 3; + + /** + * Guard to check whether we should be defending more aggressively + * + * @param event the FSM event + * + * @return whether we should be defending more aggressively + */ + bool shouldDefendAggressively(const Update& event); + /** * Action to identify all immediate enemy threats and assign * defenders to block enemy shots and passes * * @param event the FSM event */ - void defendAgainstThreats(const Update& event); + void blockShots(const Update& event); + + /** + * Action to shadow the primary enemy threat and assign + * extra defenders to block enemy shots and passes + * + * @param event the FSM event + */ + void shadowAndBlockShots(const Update& event); + + /** + * Helper function to update crease and pass defenders to defend + * against specified threats + * + * @param event the FSM event + * @param enemy_threats the enemy threats to defend against + */ + void updateCreaseAndPassDefenders(const Update& event, + const std::vector& enemy_threats); + + /** + * Helper function to update shadowers to shadow specified threats + * + * @param event the FSM event + * @param threats_to_shadow the enemy threats to shadow + */ + void updateShadowers(const Update& event, + const std::vector& threats_to_shadow); + + + /** + * Helper function to set up shadow enemy tactic vector members + * + * @param num_shadowers the number of shadow enemy tactics to set + */ + void setUpShadowers(unsigned int num_shadowers); + + /** + * Helper function to return the next tactics + * + * @param event the FSM event + */ + void setTactics(const Update& event); + + auto operator()() { using namespace boost::sml; DEFINE_SML_STATE(DefenseState) + DEFINE_SML_STATE(AggressiveDefenseState) DEFINE_SML_EVENT(Update) - DEFINE_SML_ACTION(defendAgainstThreats) + DEFINE_SML_GUARD(shouldDefendAggressively) + + DEFINE_SML_ACTION(blockShots) + DEFINE_SML_ACTION(shadowAndBlockShots) return make_transition_table( // src_state + event [guard] / action = dest_state - *DefenseState_S + Update_E / defendAgainstThreats_A = DefenseState_S, - X + Update_E = X); + + *DefenseState_S + Update_E[shouldDefendAggressively_G] / + shadowAndBlockShots_A = AggressiveDefenseState_S, + DefenseState_S + Update_E / blockShots_A = DefenseState_S, + AggressiveDefenseState_S + + Update_E[!shouldDefendAggressively_G] / blockShots_A = DefenseState_S, + AggressiveDefenseState_S + Update_E / shadowAndBlockShots_A = + AggressiveDefenseState_S, + X + Update_E = X); } }; diff --git a/src/software/ai/hl/stp/play/defense/defense_play_fsm_test.cpp b/src/software/ai/hl/stp/play/defense/defense_play_fsm_test.cpp index dbb8bc8372..fd6fcaa087 100644 --- a/src/software/ai/hl/stp/play/defense/defense_play_fsm_test.cpp +++ b/src/software/ai/hl/stp/play/defense/defense_play_fsm_test.cpp @@ -23,5 +23,6 @@ TEST(DefensePlayFSMTest, test_transitions) [](InterPlayCommunication comm) {}))); // DefensePlayFSM always stays in the DefenseState - EXPECT_TRUE(fsm.is(boost::sml::state)); + EXPECT_TRUE(fsm.is(boost::sml::state) || + fsm.is(boost::sml::state)); } diff --git a/src/software/ai/hl/stp/play/defense/defense_play_test.py b/src/software/ai/hl/stp/play/defense/defense_play_test.py index 722a74b97d..d33c0a6b3c 100644 --- a/src/software/ai/hl/stp/play/defense/defense_play_test.py +++ b/src/software/ai/hl/stp/play/defense/defense_play_test.py @@ -5,10 +5,89 @@ import software.python_bindings as tbots_cpp from proto.play_pb2 import Play, PlayName from software.simulated_tests.ball_enters_region import * +from software.simulated_tests.friendly_has_ball_possession import ( + FriendlyEventuallyHasBallPossession, +) from proto.message_translation.tbots_protobuf import create_world_state from proto.ssl_gc_common_pb2 import Team +@pytest.mark.parametrize( + "blue_bots,yellow_bots", + [ + ( + [ + tbots_cpp.Point(-3, 1.5), + tbots_cpp.Point(-3, 0.5), + tbots_cpp.Point(-3, -0.5), + tbots_cpp.Point(-3, -1.5), + tbots_cpp.Point(-3, 1), + tbots_cpp.Point(-0.75, 0), + ], + [ + tbots_cpp.Point(3, 0), + tbots_cpp.Point(1, -1), + tbots_cpp.Point(1, 0), + tbots_cpp.Point(2, -1.25), + tbots_cpp.Point(2, 1), + ], + ) + ], +) +def test_defense_play_ball_steal(simulated_test_runner, blue_bots, yellow_bots): + def setup(*args): + # Starting point must be Point + ball_initial_pos = tbots_cpp.Point(0.93, 0) + # Placement point must be Vector2 to work with game controller + tbots_cpp.Point(-3, -2) + + # Game Controller Setup + simulated_test_runner.gamecontroller.send_gc_command( + gc_command=Command.Type.STOP, team=Team.UNKNOWN + ) + simulated_test_runner.gamecontroller.send_gc_command( + gc_command=Command.Type.FORCE_START, team=Team.BLUE + ) + + # Force play override here + blue_play = Play() + blue_play.name = PlayName.DefensePlay + + params = AssignedTacticPlayControlParams() + + simulated_test_runner.blue_full_system_proto_unix_io.send_proto(Play, blue_play) + simulated_test_runner.yellow_full_system_proto_unix_io.send_proto( + AssignedTacticPlayControlParams, params + ) + + # Create world state + simulated_test_runner.simulator_proto_unix_io.send_proto( + WorldState, + create_world_state( + yellow_robot_locations=yellow_bots, + blue_robot_locations=blue_bots, + ball_location=ball_initial_pos, + ball_velocity=tbots_cpp.Vector(0, 0), + ), + ) + + simulated_test_runner.run_test( + setup=setup, + params=[0, 1, 2, 3, 4], # The aggregate test runs 5 times + inv_always_validation_sequence_set=[[]], + inv_eventually_validation_sequence_set=[[]], + ag_always_validation_sequence_set=[ + [ + BallNeverEntersRegion( + regions=[tbots_cpp.Field.createSSLDivisionBField().friendlyGoal()] + ) + ] + ], + ag_eventually_validation_sequence_set=[[]], + test_timeout_s=12, + ) + + @pytest.mark.parametrize( "blue_bots,yellow_bots", [ @@ -81,8 +160,10 @@ def setup(*args): ) ] ], - ag_eventually_validation_sequence_set=[[]], - test_timeout_s=30, + ag_eventually_validation_sequence_set=[ + [FriendlyEventuallyHasBallPossession(tolerance=0.05)] + ], + test_timeout_s=6, ) diff --git a/src/software/ai/hl/stp/stp_tactic_assignment_test.cpp b/src/software/ai/hl/stp/stp_tactic_assignment_test.cpp index 060ba158cb..5f02989c0d 100644 --- a/src/software/ai/hl/stp/stp_tactic_assignment_test.cpp +++ b/src/software/ai/hl/stp/stp_tactic_assignment_test.cpp @@ -365,7 +365,7 @@ TEST_F(STPTacticAssignmentTest, // The destination of the move_tactic is relatively close to the robot positions, so // the cost of assigning any robot to the move_tactic should be less than the - // halt_tactics + // stop_tactics move_tactic_1->updateControlParams(Point(0, 0), Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT); diff --git a/src/software/ai/hl/stp/tactic/shadow_enemy/BUILD b/src/software/ai/hl/stp/tactic/shadow_enemy/BUILD index 3ec178b5f3..78df2b247e 100644 --- a/src/software/ai/hl/stp/tactic/shadow_enemy/BUILD +++ b/src/software/ai/hl/stp/tactic/shadow_enemy/BUILD @@ -1,5 +1,7 @@ package(default_visibility = ["//visibility:public"]) +load("@simulated_tests_deps//:requirements.bzl", "requirement") + cc_library( name = "shadow_enemy_tactic", srcs = [ @@ -15,6 +17,7 @@ cc_library( "//software/ai/evaluation:enemy_threat", "//software/ai/hl/stp/tactic", "//software/ai/hl/stp/tactic/move:move_tactic", + "//software/geom/algorithms", "//software/logger", ], ) @@ -28,19 +31,3 @@ cc_test( "//software/test_util", ], ) - -cc_test( - name = "shadow_enemy_tactic_test", - srcs = ["shadow_enemy_tactic_test.cpp"], - deps = [ - ":shadow_enemy_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/geom:triangle", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/terminating_validation_functions", - "//software/simulated_tests/validation:validation_function", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) diff --git a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm.cpp b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm.cpp index 407bde1ff0..b24066861c 100644 --- a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm.cpp @@ -1,6 +1,7 @@ #include "software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm.h" #include "software/ai/hl/stp/tactic/move_primitive.h" +#include "software/geom/algorithms/distance.h" Point ShadowEnemyFSM::findBlockPassPoint(const Point &ball_position, const Robot &shadowee, @@ -38,15 +39,40 @@ Point ShadowEnemyFSM::findBlockShotPoint(const Robot &robot, const Field &field, bool ShadowEnemyFSM::enemyThreatHasBall(const Update &event) { std::optional enemy_threat_opt = event.control_params.enemy_threat; + if (enemy_threat_opt.has_value()) { - return enemy_threat_opt.value().has_ball; - }; - LOG(WARNING) << "Enemy threat not initialized for robot " << event.common.robot.id() - << "\n"; + bool near_ball = distance(event.common.world_ptr->ball().position(), + enemy_threat_opt.value().robot.position()) < 0.22; + + return near_ball; + } return false; } +bool ShadowEnemyFSM::blockedShot(const Update &event) +{ + auto ball_position = event.common.world_ptr->ball().position(); + Ray shot_block_direction(ball_position, + event.common.robot.position() - ball_position); + Segment goalLine(event.common.world_ptr->field().friendlyGoal().negXNegYCorner(), + event.common.world_ptr->field().friendlyGoal().negXPosYCorner()); + bool ball_blocked = intersects(goalLine, shot_block_direction); + return ball_blocked; +} + + +bool ShadowEnemyFSM::contestedBall(const Update &event) +{ + // OK so basically you need to change thresholds for how close + ////in robot.h there is a isneardribbler function u can use instead of this breakbeams + /// stuff + bool robot_contesting = distance(event.common.world_ptr->ball().position(), + event.common.robot.position()) < 0.08; + + return robot_contesting; +} + void ShadowEnemyFSM::blockPass(const Update &event) { std::optional enemy_threat_opt = event.control_params.enemy_threat; @@ -111,7 +137,7 @@ void ShadowEnemyFSM::blockShot(const Update &event, processEvent(MoveFSM::Update(control_params, event.common)); } -void ShadowEnemyFSM::stealAndChip(const Update &event) +void ShadowEnemyFSM::goAndSteal(const Update &event) { auto ball_position = event.common.world_ptr->ball().position(); auto face_ball_orientation = @@ -122,5 +148,21 @@ void ShadowEnemyFSM::stealAndChip(const Update &event) TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, TbotsProto::DribblerMode::MAX_FORCE, TbotsProto::BallCollisionType::ALLOW, - AutoChipOrKick{AutoChipOrKickMode::AUTOCHIP, YEET_CHIP_DISTANCE_METERS})); + AutoChipOrKick{AutoChipOrKickMode::OFF, 0.0})); +} + +void ShadowEnemyFSM::stealAndPull(const Update &event) +{ + auto ball_position = event.common.world_ptr->ball().position(); + auto face_ball_orientation = + (ball_position - event.common.robot.position()).orientation(); + auto pull_to_here = + (event.common.robot.position() - ball_position) * 2 + ball_position; + + event.common.set_primitive(std::make_unique( + event.common.robot, pull_to_here, face_ball_orientation, + TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, + TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, + TbotsProto::DribblerMode::MAX_FORCE, TbotsProto::BallCollisionType::ALLOW, + AutoChipOrKick{AutoChipOrKickMode::OFF, 0.0})); } diff --git a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm.h b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm.h index 9048f0440f..9a07ec1e72 100644 --- a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm.h +++ b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm.h @@ -5,13 +5,15 @@ #include "software/ai/hl/stp/tactic/move/move_fsm.h" #include "software/ai/hl/stp/tactic/tactic.h" #include "software/geom/algorithms/distance.h" +#include "software/geom/algorithms/intersects.h" #include "software/logger/logger.h" struct ShadowEnemyFSM { public: class BlockPassState; - class StealAndChipState; + class GoAndStealState; + class StealAndPullState; // this struct defines the unique control parameters that the ShadowEnemyFSM requires // in its update @@ -29,11 +31,6 @@ struct ShadowEnemyFSM DEFINE_TACTIC_UPDATE_STRUCT_WITH_CONTROL_AND_COMMON_PARAMS - // Distance to chip the ball when trying to yeet it - // TODO (#1878): Replace this with a more intelligent chip distance system - static constexpr double YEET_CHIP_DISTANCE_METERS = 2.0; - - /** * Calculates the point to block the pass to the robot we are shadowing * @@ -69,6 +66,24 @@ struct ShadowEnemyFSM */ bool enemyThreatHasBall(const Update &event); + /** + * Guard that checks if we may have contested the ball + * + * @param event ShadowEnemyFSM::Update + * + * @return if we are within dribbling range of ball + */ + bool contestedBall(const Update &event); + + /** + * Guard that checks if we have essentially blocked the shot + * + * @param event ShadowEnemyFSM::Update + * + * @return if we are blocking a shot + */ + bool blockedShot(const Update &event); + /** * Action to block the pass to our shadowee * @@ -87,13 +102,22 @@ struct ShadowEnemyFSM boost::sml::back::process processEvent); /** - * Action to steal and chip the ball + * Action to go to steal the ball * - * Steal the ball if enemy threat is close enough and chip the ball away + * Go to steal the ball if enemy threat is close enough and chip the ball away * * @param event ShadowEnemyFSM::Update */ - void stealAndChip(const Update &event); + void goAndSteal(const Update &event); + + /** + * Action to pull the ball + * + * Attempt to pull the ball away if within roller + * + * @param event ShadowEnemyFSM::Update + */ + void stealAndPull(const Update &event); auto operator()() { @@ -101,22 +125,34 @@ struct ShadowEnemyFSM DEFINE_SML_STATE(MoveFSM) DEFINE_SML_STATE(BlockPassState) - DEFINE_SML_STATE(StealAndChipState) + DEFINE_SML_STATE(GoAndStealState) + DEFINE_SML_STATE(StealAndPullState) + DEFINE_SML_EVENT(Update) DEFINE_SML_GUARD(enemyThreatHasBall) + DEFINE_SML_GUARD(contestedBall) + DEFINE_SML_GUARD(blockedShot) + DEFINE_SML_ACTION(blockPass) - DEFINE_SML_ACTION(stealAndChip) + DEFINE_SML_ACTION(goAndSteal) + DEFINE_SML_ACTION(stealAndPull) DEFINE_SML_SUB_FSM_UPDATE_ACTION(blockShot, MoveFSM) return make_transition_table( // src_state + event [guard] / action = dest_state *MoveFSM_S + Update_E[!enemyThreatHasBall_G] / blockPass_A = BlockPassState_S, - MoveFSM_S + Update_E / blockShot_A, MoveFSM_S = StealAndChipState_S, + MoveFSM_S + Update_E[blockedShot_G] / goAndSteal_A = GoAndStealState_S, + MoveFSM_S + Update_E / blockShot_A, MoveFSM_S = GoAndStealState_S, BlockPassState_S + Update_E[!enemyThreatHasBall_G] / blockPass_A, BlockPassState_S + Update_E[enemyThreatHasBall_G] / blockShot_A = MoveFSM_S, - StealAndChipState_S + Update_E[enemyThreatHasBall_G] / stealAndChip_A, - StealAndChipState_S + Update_E[!enemyThreatHasBall_G] / blockPass_A = X, + GoAndStealState_S + + Update_E[enemyThreatHasBall_G && !contestedBall_G] / goAndSteal_A, + GoAndStealState_S + Update_E[enemyThreatHasBall_G && contestedBall_G] / + goAndSteal_A = StealAndPullState_S, + GoAndStealState_S + Update_E[!enemyThreatHasBall_G] / blockPass_A = X, + StealAndPullState_S + Update_E[enemyThreatHasBall_G] / stealAndPull_A, + StealAndPullState_S + Update_E[!enemyThreatHasBall_G] / blockPass_A = X, X + Update_E[!enemyThreatHasBall_G] / blockPass_A = BlockPassState_S, X + Update_E[enemyThreatHasBall_G] / blockShot_A = MoveFSM_S, X + Update_E / SET_STOP_PRIMITIVE_ACTION = X); diff --git a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm_test.cpp b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm_test.cpp index 65b2e9e822..a754da0537 100644 --- a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm_test.cpp +++ b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_fsm_test.cpp @@ -116,16 +116,23 @@ TEST(ShadowEnemyFSMTest, test_transitions) fsm.process_event(ShadowEnemyFSM::Update( {enemy_threat, 0.5}, TacticUpdate(shadower, world, [](std::shared_ptr) {}))); - EXPECT_TRUE(fsm.is(boost::sml::state)); - // Shadowee still has possession of the ball - // Robot should continue to try and steal and chip the ball + EXPECT_TRUE(fsm.is(boost::sml::state)); + // Shadower is now near the shadowee + // Robot should try and steal and pull the ball + shadower.updateState( + RobotState(Point(0, -1.97), Vector(), + (world->ball().position() - position_to_block).orientation(), + AngularVelocity::zero()), + Timestamp::fromSeconds(0)); + + fsm.process_event(ShadowEnemyFSM::Update( {enemy_threat, 0.5}, TacticUpdate(shadower, world, [](std::shared_ptr) {}))); - EXPECT_TRUE(fsm.is(boost::sml::state)); + EXPECT_TRUE(fsm.is(boost::sml::state)); - // Either the ball has been stolen and chipped by our robot or the + // Either the ball has been stolen by our robot or at least the // enemy threat has kicked the ball // Tactic is done enemy_threat.has_ball = false; diff --git a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic_test.cpp b/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic_test.cpp deleted file mode 100644 index 09959c6705..0000000000 --- a/src/software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic_test.cpp +++ /dev/null @@ -1,219 +0,0 @@ -#include "software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic.h" - -#include - -#include - -#include "software/geom/triangle.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class ShadowEnemyTacticTest : public SimulatedErForceSimPlayTestFixture -{ - void SetUp() override - { - SimulatedErForceSimPlayTestFixture::SetUp(); - } - - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); - std::set motion_constraints = { - TbotsProto::MotionConstraint::ENEMY_DEFENSE_AREA}; -}; - -TEST_F(ShadowEnemyTacticTest, test_block_pass) -{ - Robot shadower(0, Point(-2, 0), Vector(0, 0), Angle::zero(), AngularVelocity::zero(), - Timestamp::fromSeconds(0)); - Robot shadowee(1, Point(0, -2), Vector(0, 0), Angle::quarter(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); - Robot enemy(2, Point(0, 2), Vector(0, 0), Angle::threeQuarter(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); - - - - EnemyThreat enemy_threat{shadowee, false, Angle::zero(), std::nullopt, - std::nullopt, 1, enemy}; - - auto friendly_robots = { - RobotStateWithId{.id = 0, .robot_state = shadower.currentState()}}; - auto enemy_robots = { - RobotStateWithId{.id = 0, - .robot_state = RobotState(Point(4, 0), Vector(), Angle::zero(), - AngularVelocity::zero())}, - RobotStateWithId{.id = 1, .robot_state = shadowee.currentState()}, - RobotStateWithId{.id = 2, .robot_state = enemy.currentState()}}; - - - BallState ball_state(Point(0, 2), Vector(0, 0)); - auto tactic = std::make_shared(); - tactic->updateControlParams(enemy_threat, 2); - setTactic(0, tactic, motion_constraints); - - std::vector terminating_validation_functions = { - [tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - // As the shadowee is located at (0,-2) and the enemy robot that - // has the ball is located at (0,2), we would like to block the pass - // with a shadow distance of 2 - Point destination = Point(0, 0); - robotAtPosition(0, world_ptr, destination, 0.01, yield); - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(5)); -} - -TEST_F(ShadowEnemyTacticTest, test_block_pass_if_enemy_does_not_have_ball) -{ - Robot shadower(0, Point(-2, 0), Vector(0, 0), Angle::zero(), AngularVelocity::zero(), - Timestamp::fromSeconds(0)); - Robot shadowee(1, Point(0, -2), Vector(0, 0), Angle::quarter(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); - Robot enemy(2, Point(0, 2), Vector(0, 0), Angle::threeQuarter(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); - - - - EnemyThreat enemy_threat{shadowee, false, Angle::zero(), std::nullopt, - std::nullopt, 1, enemy}; - - auto friendly_robots = { - RobotStateWithId{.id = 0, .robot_state = shadower.currentState()}}; - auto enemy_robots = { - RobotStateWithId{.id = 0, - .robot_state = RobotState(Point(4, 0), Vector(), Angle::zero(), - AngularVelocity::zero())}, - RobotStateWithId{.id = 1, .robot_state = shadowee.currentState()}, - RobotStateWithId{.id = 2, .robot_state = enemy.currentState()}}; - - - BallState ball_state(Point(3, 0), Vector(0, 0)); - auto tactic = std::make_shared(); - tactic->updateControlParams(enemy_threat, 1.5); - setTactic(0, tactic, motion_constraints); - - std::vector terminating_validation_functions = { - [this, tactic, shadowee](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - // As the shadowee is located at (0,-2) and the ball is located at (3,0), - // we would like to block the pass with a shadow distance of 1.5 - Vector pass = Point(3, 0) - shadowee.position(); - Point destination = shadowee.position() + pass.normalize(1.5); - robotAtPosition(0, world_ptr, destination, 0.01, yield); - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(5)); -} - -TEST_F(ShadowEnemyTacticTest, test_block_net_then_steal_and_chip) -{ - Robot shadower(0, Point(-2, 0), Vector(0, 0), Angle::zero(), AngularVelocity::zero(), - Timestamp::fromSeconds(0)); - Robot shadowee(1, Point(0, -2), Vector(0, 0), Angle::fromDegrees(135), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); - Robot enemy(2, Point(0, 2), Vector(0, 0), Angle::threeQuarter(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); - - - EnemyThreat enemy_threat{shadowee, true, Angle::zero(), std::nullopt, - std::nullopt, 1, enemy}; - - auto friendly_robots = { - RobotStateWithId{.id = 0, .robot_state = shadower.currentState()}}; - auto enemy_robots = { - RobotStateWithId{.id = 0, - .robot_state = RobotState(Point(4, 0), Vector(), Angle::zero(), - AngularVelocity::zero())}, - RobotStateWithId{.id = 1, .robot_state = shadowee.currentState()}, - RobotStateWithId{.id = 2, .robot_state = enemy.currentState()}}; - - - BallState ball_state(Point(0, -1.75), Vector(0, 0)); - auto tactic = std::make_shared(); - tactic->updateControlParams(enemy_threat, 2); - setTactic(0, tactic, motion_constraints); - - std::vector terminating_validation_functions = { - [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - // We compose a triangle consisting of the friendly goal posts - // and the ball position. If our robot is in this triangle, then - // it is blocking a possible shot on net - Triangle shotTriangle{world_ptr->field().friendlyGoalpostPos(), - world_ptr->field().friendlyGoalpostNeg(), - world_ptr->ball().position()}; - robotInPolygon(shotTriangle, 1, world_ptr, yield); - }, - [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - // As our friendly robot tries to steal and chip the ball, - // it should chip the ball in the same direction is it - // heading towards the ball - Vector chip = world_ptr->ball().position() - - world_ptr->friendlyTeam().getRobotById(0).value().position(); - ballKicked(chip.orientation(), world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(5)); -} - -TEST_F(ShadowEnemyTacticTest, test_block_net_if_enemy_threat_is_null) -{ - Robot shadower(0, Point(-2, 0), Vector(0, 0), Angle::zero(), AngularVelocity::zero(), - Timestamp::fromSeconds(0)); - Robot shadowee(1, Point(0, -2), Vector(0, 0), Angle::fromDegrees(135), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); - Robot enemy(2, Point(0, 2), Vector(0, 0), Angle::threeQuarter(), - AngularVelocity::zero(), Timestamp::fromSeconds(0)); - - auto friendly_robots = { - RobotStateWithId{.id = 0, .robot_state = shadower.currentState()}}; - auto enemy_robots = { - RobotStateWithId{.id = 0, - .robot_state = RobotState(Point(4, 0), Vector(), Angle::zero(), - AngularVelocity::zero())}, - RobotStateWithId{.id = 1, .robot_state = shadowee.currentState()}, - RobotStateWithId{.id = 2, .robot_state = enemy.currentState()}}; - - - BallState ball_state(Point(0, -1.75), Vector(0, 0)); - auto tactic = std::make_shared(); - tactic->updateControlParams(std::nullopt, 2); - setTactic(0, tactic, motion_constraints); - - std::vector terminating_validation_functions = { - [tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) { - // We compose a triangle consisting of the friendly goal posts - // and the ball position. If our robot is in this triangle, then - // it is blocking a possible shot on net - Triangle shotTriangle{world_ptr->field().friendlyGoalpostPos(), - world_ptr->field().friendlyGoalpostNeg(), - world_ptr->ball().position()}; - robotInPolygon(shotTriangle, 1, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(5)); -} diff --git a/src/software/python_bindings.cpp b/src/software/python_bindings.cpp index 4d17edf6e4..f5ab61138f 100644 --- a/src/software/python_bindings.cpp +++ b/src/software/python_bindings.cpp @@ -311,6 +311,7 @@ PYBIND11_MODULE(python_bindings, m) m.def("createPointProto", &createPointProto); m.def("createPolygonProto", &createPolygonProto); m.def("createCircleProto", &createCircleProto); + m.def("createAngleProto", &createAngleProto); m.def("createVectorProto", &createVectorProto); m.def("createSegmentProto", &createSegmentProto); m.def("createStadiumProto", &createStadiumProto); diff --git a/src/software/thunderscope/thunderscope_config.py b/src/software/thunderscope/thunderscope_config.py index a0a0ecf895..c8a552bf63 100644 --- a/src/software/thunderscope/thunderscope_config.py +++ b/src/software/thunderscope/thunderscope_config.py @@ -475,7 +475,11 @@ def configure_replay_view( replay=True, replay_log=blue_replay_log, visualization_buffer_size=visualization_buffer_size, - extra_widgets=[], + extra_widgets=[ + configure_robot_view_fullsystem( + proto_unix_io_map[ProtoUnixIOTypes.BLUE] + ) + ], frame_swap_counter=FrameTimeCounter(), refresh_counter=blue_refresh_func_counter, ), @@ -498,7 +502,11 @@ def configure_replay_view( replay=True, replay_log=yellow_replay_log, visualization_buffer_size=visualization_buffer_size, - extra_widgets=[], + extra_widgets=[ + configure_robot_view_fullsystem( + proto_unix_io_map[ProtoUnixIOTypes.YELLOW] + ) + ], frame_swap_counter=FrameTimeCounter(), refresh_counter=yellow_refresh_func_counter, ), diff --git a/src/software/world/robot.cpp b/src/software/world/robot.cpp index fd9efd3acd..b2befeff6c 100644 --- a/src/software/world/robot.cpp +++ b/src/software/world/robot.cpp @@ -97,6 +97,7 @@ AngularVelocity Robot::angularVelocity() const bool Robot::isNearDribbler(const Point &test_point, double TOLERANCE) const { + // The 0.06 is due to enemies losing the ball causing the hasball function to fail const double POSSESSION_THRESHOLD_METERS = DIST_TO_FRONT_OF_ROBOT_METERS + TOLERANCE; Vector vector_to_test_point = test_point - position();