diff --git a/src/software/ai/hl/stp/play/BUILD b/src/software/ai/hl/stp/play/BUILD index 4980c32a0d..45ec31107d 100644 --- a/src/software/ai/hl/stp/play/BUILD +++ b/src/software/ai/hl/stp/play/BUILD @@ -6,40 +6,6 @@ package(default_visibility = ["//visibility:public"]) # "factory" design pattern to work are linked in # https://www.bfilipek.com/2018/02/static-vars-static-lib.html -cc_library( - name = "kickoff_enemy_play", - srcs = ["kickoff_enemy_play.cpp"], - hdrs = ["kickoff_enemy_play.h"], - deps = [ - ":play", - "//shared:constants", - "//software/ai/evaluation:enemy_threat", - "//software/ai/evaluation:possession", - "//software/ai/hl/stp/tactic/goalie:goalie_tactic", - "//software/ai/hl/stp/tactic/move:move_tactic", - "//software/ai/hl/stp/tactic/shadow_enemy:shadow_enemy_tactic", - "//software/logger", - "//software/util/generic_factory", - ], - alwayslink = True, -) - -cc_library( - name = "kickoff_friendly_play", - srcs = ["kickoff_friendly_play.cpp"], - hdrs = ["kickoff_friendly_play.h"], - deps = [ - ":play", - "//shared:constants", - "//software/ai/evaluation:enemy_threat", - "//software/ai/hl/stp/tactic/chip:chip_tactic", - "//software/ai/hl/stp/tactic/move:move_tactic", - "//software/logger", - "//software/util/generic_factory", - ], - alwayslink = True, -) - cc_library( name = "shoot_or_chip_play", srcs = ["shoot_or_chip_play.cpp"], @@ -105,8 +71,6 @@ cc_library( cc_library( name = "all_plays", deps = [ - ":kickoff_enemy_play", - ":kickoff_friendly_play", ":shoot_or_chip_play", ":stop_play", "//software/ai/hl/stp/play/ball_placement:ball_placement_play", @@ -116,6 +80,8 @@ cc_library( "//software/ai/hl/stp/play/enemy_free_kick:enemy_free_kick_play", "//software/ai/hl/stp/play/example:example_play", "//software/ai/hl/stp/play/free_kick:free_kick_play", + "//software/ai/hl/stp/play/kickoff_friendly:kickoff_friendly_play", + "//software/ai/hl/stp/play/kickoff_enemy:kickoff_enemy_play", "//software/ai/hl/stp/play/halt_play", "//software/ai/hl/stp/play/hardware_challenge_plays:dribbling_parcour_play", "//software/ai/hl/stp/play/hardware_challenge_plays:pass_endurance_play", @@ -128,54 +94,9 @@ cc_library( ], ) -cc_test( - name = "kickoff_friendly_play_cpp_test", - srcs = ["kickoff_friendly_play_test.cpp"], - deps = [ - "//shared/test_util:tbots_gtest_main", - "//software/ai/hl/stp/play:kickoff_friendly_play", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/non_terminating_validation_functions", - "//software/simulated_tests/terminating_validation_functions", - "//software/simulated_tests/validation:validation_function", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) -cc_test( - name = "kickoff_enemy_play_cpp_test", - srcs = ["kickoff_enemy_play_test.cpp"], - deps = [ - "//shared/test_util:tbots_gtest_main", - "//software/ai/hl/stp/play:kickoff_enemy_play", - "//software/geom/algorithms", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/non_terminating_validation_functions", - "//software/simulated_tests/terminating_validation_functions", - "//software/simulated_tests/validation:validation_function", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) -py_test( - name = "kickoff_play_test", - srcs = [ - "kickoff_play_test.py", - ], - # TODO (#2619) Remove tag to run in parallel - tags = [ - "exclusive", - ], - deps = [ - "//software:conftest", - "//software/simulated_tests:validation", - requirement("pytest"), - ], -) + cc_test( name = "stop_play_test", @@ -277,3 +198,19 @@ py_test( requirement("pytest"), ], ) + +py_test( + name = "kickoff_play_test", + srcs = [ + "kickoff_play_test.py", + ], + # TODO (#2619) Remove tag to run in parallel + tags = [ + "exclusive", + ], + deps = [ + "//software:conftest", + "//software/simulated_tests:validation", + requirement("pytest"), + ], +) diff --git a/src/software/ai/hl/stp/play/enemy_free_kick/enemy_free_kick_play_test.py b/src/software/ai/hl/stp/play/enemy_free_kick/enemy_free_kick_play_test.py index c32e4c3dff..61a003eb1b 100644 --- a/src/software/ai/hl/stp/play/enemy_free_kick/enemy_free_kick_play_test.py +++ b/src/software/ai/hl/stp/play/enemy_free_kick/enemy_free_kick_play_test.py @@ -94,6 +94,10 @@ ), ], ) +# TODO: #3503 +@pytest.mark.skip( + "Disabling this test because OrValidation is passed both an always validation and eventually validation" +) def test_enemy_free_kick_play( simulated_test_runner, blue_bots, yellow_bots, ball_initial_pos ): diff --git a/src/software/ai/hl/stp/play/example/example_play_test.py b/src/software/ai/hl/stp/play/example/example_play_test.py index ef5560feb1..6dfb008106 100644 --- a/src/software/ai/hl/stp/play/example/example_play_test.py +++ b/src/software/ai/hl/stp/play/example/example_play_test.py @@ -82,10 +82,10 @@ def setup(*args): inv_eventually_validation_sequence_set=[ [ NumberOfRobotsEventuallyEntersRegion( - region=tbots_cpp.Circle(ball_initial_pos, 1.1), req_robot_cnt=6 + regions=[tbots_cpp.Circle(ball_initial_pos, 1.1)], req_robot_cnt=6 ), NumberOfRobotsEventuallyExitsRegion( - region=tbots_cpp.Circle(ball_initial_pos, 0.9), req_robot_cnt=6 + regions=[tbots_cpp.Circle(ball_initial_pos, 0.9)], req_robot_cnt=6 ), ] ], diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/BUILD b/src/software/ai/hl/stp/play/kickoff_enemy/BUILD new file mode 100644 index 0000000000..f9b9170a53 --- /dev/null +++ b/src/software/ai/hl/stp/play/kickoff_enemy/BUILD @@ -0,0 +1,60 @@ +package(default_visibility = ["//visibility:public"]) + +load("@simulated_tests_deps//:requirements.bzl", "requirement") + +cc_library( + name = "kickoff_enemy_play", + srcs = [ + "kickoff_enemy_play.cpp", + "kickoff_enemy_play_fsm.cpp" + ], + hdrs = [ + "kickoff_enemy_play.h", + "kickoff_enemy_play_fsm.h", + ], + deps = [ + "//software/ai/hl/stp/play", + "//shared:constants", + "//software/ai/evaluation:enemy_threat", + "//software/ai/evaluation:possession", + "//software/ai/hl/stp/tactic/goalie:goalie_tactic", + "//software/ai/hl/stp/tactic/move:move_tactic", + "//software/ai/hl/stp/tactic/shadow_enemy:shadow_enemy_tactic", + "//software/logger", + "//software/util/generic_factory", + ], + alwayslink = True, +) + +py_test( + name = "kickoff_enemy_play_test", + srcs = [ + "kickoff_enemy_play_test.py", + ], + # TODO (#2619) Remove tag to run in parallel + tags = [ + "exclusive", + ], + deps = [ + "//software:conftest", + "//software/simulated_tests:validation", + requirement("pytest"), + ], +) + +cc_test( + name = "kickoff_enemy_play_cpp_test", + srcs = ["kickoff_enemy_play_test.cpp"], + deps = [ + "//shared/test_util:tbots_gtest_main", + "//software/ai/hl/stp/play:kickoff_enemy_play", + "//software/geom/algorithms", + "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", + "//software/simulated_tests/non_terminating_validation_functions", + "//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/play/kickoff_enemy/kickoff_enemy_play.cpp b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play.cpp new file mode 100644 index 0000000000..34e222f646 --- /dev/null +++ b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play.cpp @@ -0,0 +1,35 @@ +#include "software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play.h" + +#include "shared/constants.h" +#include "software/util/generic_factory/generic_factory.h" + +KickoffEnemyPlay::KickoffEnemyPlay(TbotsProto::AiConfig config) + : Play(config, true), fsm{KickoffEnemyPlayFSM{config}}, control_params{} +{ +} + +void KickoffEnemyPlay::getNextTactics(TacticCoroutine::push_type &yield, + const WorldPtr &world_ptr) +{ + // Does not get called. + while (true) + { + yield({{}}); + } +} + +void KickoffEnemyPlay::updateTactics(const PlayUpdate &play_update) +{ + fsm.process_event(KickoffEnemyPlayFSM::Update(control_params, play_update)); +} + +std::vector KickoffEnemyPlay::getState() +{ + std::vector state; + state.emplace_back(objectTypeName(*this) + " - " + getCurrentFullStateName(fsm)); + return state; +} + + +// Register this play in the genericFactory +static TGenericFactory factory; diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play.h b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play.h new file mode 100644 index 0000000000..f41d898387 --- /dev/null +++ b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play.h @@ -0,0 +1,29 @@ +#pragma once + +#include "proto/parameters.pb.h" +#include "software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_fsm.h" +#include "software/ai/hl/stp/play/play.h" + +/** + * A play that runs when its currently the enemy kick off. + */ + +class KickoffEnemyPlay : public Play +{ +public: + /** + * Creates an enemy kickoff play + * + * @param ai_config the play config for this play + */ + KickoffEnemyPlay(TbotsProto::AiConfig config); + + void getNextTactics(TacticCoroutine::push_type &yield, + const WorldPtr &world_ptr) override; + void updateTactics(const PlayUpdate &play_update) override; + std::vector getState() override; + +private: + FSM fsm; + KickoffEnemyPlayFSM::ControlParams control_params; +}; \ No newline at end of file diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_fsm.cpp b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_fsm.cpp new file mode 100644 index 0000000000..fb7229f112 --- /dev/null +++ b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_fsm.cpp @@ -0,0 +1,165 @@ +#include "software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_fsm.h" + +KickoffEnemyPlayFSM::KickoffEnemyPlayFSM(const TbotsProto::AiConfig &ai_config) + : ai_config(ai_config), + shadow_enemy_tactics({ + std::make_shared(), + std::make_shared() + }), + move_tactics({ + std::make_shared(), // for robot 1 + std::make_shared(), // for robot 2 + std::make_shared(), // for robot 3 + std::make_shared(), // for robot 4 + std::make_shared() // for robot 5 + }) +{ + +} + +void KickoffEnemyPlayFSM::createKickoffSetupPositions(const WorldPtr &world_ptr) +{ + // these positions are picked according to the followicreateKickoffSetupPositions();ng slide + // https://images.slideplayer.com/32/9922349/slides/slide_2.jpg + // since we only have 6 robots at the maximum, 3 robots will shadow threats + // up front, 1 robot is dedicated as the goalie, and the other 2 robots will defend + // either post (as show in the image) + // + // Positions 1,2 are the most important, 3,4,5 are a fallback + // if there aren't as many threats to shadow. Robots will be assigned + // to those positions in order of priority. The 5 positions shown below + // are in the same order as in the defense_position vector. + // + // +--------------------+--------------------+ + // | | | + // | | | + // | | | + // +--+ 2 4 | +--+ + // | | | | | + // | | +-+-+ | | + // | | 3 | | | | + // | | +-+-+ | | + // | | | | | + // +--+ 1 5 | +--+ + // | | | + // | | | + // | | | + // +--------------------+--------------------+ + if (!kickoff_setup_positions.empty()) { + return; + } + + kickoff_setup_positions = { + Point(world_ptr->field().friendlyGoalpostNeg().x() + + world_ptr->field().defenseAreaXLength() + 2 * ROBOT_MAX_RADIUS_METERS, + -world_ptr->field().defenseAreaYLength() / 2.0), + Point(world_ptr->field().friendlyGoalpostPos().x() + + world_ptr->field().defenseAreaXLength() + 2 * ROBOT_MAX_RADIUS_METERS, + world_ptr->field().defenseAreaYLength() / 2.0), + Point(world_ptr->field().friendlyGoalCenter().x() + + world_ptr->field().defenseAreaXLength() + 2 * ROBOT_MAX_RADIUS_METERS, + world_ptr->field().friendlyGoalCenter().y()), + Point(-(world_ptr->field().centerCircleRadius() + 2 * ROBOT_MAX_RADIUS_METERS), + world_ptr->field().defenseAreaYLength() / 2.0), + Point(-(world_ptr->field().centerCircleRadius() + 2 * ROBOT_MAX_RADIUS_METERS), + -world_ptr->field().defenseAreaYLength() / 2.0), + }; +} + +void KickoffEnemyPlayFSM::assignShadowing( + const std::vector &enemy_threats, + PriorityTacticVector &tactics_to_run, + size_t &defense_position_index + ) +{ + const auto shadower_count = std::min(2, enemy_threats.size()); + + for (size_t i = 0; i < shadower_count; i++) + { + // Assign the first 2 robots to shadow enemies, if the enemies exist + auto enemy_threat = enemy_threats.at(i); + // Shadow with a distance slightly more than the distance from the enemy + // robot to the center line, so we are always just on our side of the + // center line + double shadow_dist = std::fabs(enemy_threat.robot.position().x()) + + 2 * ROBOT_MAX_RADIUS_METERS; + // We shadow assuming the robots do not pass so we do not try block passes + // while shadowing, since we can't go on the enemy side to block the pass + // anyway + shadow_enemy_tactics.at(i)->updateControlParams(enemy_threat, + shadow_dist); + + tactics_to_run[0].emplace_back(shadow_enemy_tactics.at(i)); + } +} + +void KickoffEnemyPlayFSM::assignDefenders( + PriorityTacticVector &tactics_to_run, + size_t &defense_position_index + ) +{ + while (defense_position_index < move_tactics.size() - 1 && defense_position_index < kickoff_setup_positions.size()) + { + move_tactics.at(defense_position_index) + ->updateControlParams(kickoff_setup_positions.at(defense_position_index), + Angle::zero()); + tactics_to_run[0].emplace_back(move_tactics.at(defense_position_index)); + defense_position_index++; + } +} + +void KickoffEnemyPlayFSM::assignGoalBlocker( + const WorldPtr &world_ptr, + PriorityTacticVector &tactics_to_run, + size_t &defense_position_index + ) +{ + move_tactics.back() + ->updateControlParams( + calculateBlockCone(world_ptr->field().friendlyGoalpostPos(), + world_ptr->field().friendlyGoalpostNeg(), + world_ptr->field().centerPoint(), + ROBOT_MAX_RADIUS_METERS), + Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, + TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE); + tactics_to_run[0].emplace_back(move_tactics.at(defense_position_index)); + defense_position_index++; +} + +void KickoffEnemyPlayFSM::kickoff(const Update &event) +{ + createKickoffSetupPositions(event.common.world_ptr); + WorldPtr world_ptr = event.common.world_ptr; + Team enemy_team = world_ptr->enemyTeam(); + PriorityTacticVector tactics_to_run = {{}}; + + // TODO: (Mathew): Minor instability with defenders and goalie when the ball and + // attacker are in the middle of the net + + // We find the nearest enemy robot closest to (0,0) then ignore it from the enemy + // team. Since the center circle is a motion constraint during enemy kickoff, the + // shadowing robot will navigate to the closest point that it can to shadow, which + // might not be ideal. (i.e robot won't block a straight shot on net) + auto robot = Team::getNearestRobot(world_ptr->enemyTeam().getAllRobots(), + world_ptr->field().centerPoint()); + if (robot.has_value()) + { + int robot_id = robot.value().id(); + enemy_team.removeRobotWithId(robot_id); + } + else + { + LOG(WARNING) << "No Robot on the Field!"; + } + + auto enemy_threats = + getAllEnemyThreats(world_ptr->field(), world_ptr->friendlyTeam(), + world_ptr->enemyTeam(), world_ptr->ball(), false); + + size_t defense_position_index = 0; + assignShadowing(enemy_threats, tactics_to_run, defense_position_index); + assignDefenders(tactics_to_run, defense_position_index); + assignGoalBlocker(world_ptr, tactics_to_run, defense_position_index); + + event.common.set_tactics(tactics_to_run); +} diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_fsm.h b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_fsm.h new file mode 100644 index 0000000000..5495d81fda --- /dev/null +++ b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_fsm.h @@ -0,0 +1,102 @@ +#pragma once + +#include "proto/parameters.pb.h" +#include "shared/constants.h" +#include "software/ai/evaluation/enemy_threat.h" +#include "software/ai/hl/stp/play/play.h" +#include "software/ai/hl/stp/play/play_fsm.h" +#include "software/ai/evaluation/possession.h" +#include "software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic.h" +#include "software/geom/algorithms/calculate_block_cone.h" +#include "software/ai/hl/stp/tactic/move/move_tactic.h" +#include "software/logger/logger.h" + + + +struct KickoffEnemyPlayFSM +{ + class SetupState; + + struct ControlParams + { + }; + + DEFINE_PLAY_UPDATE_STRUCT_WITH_CONTROL_AND_COMMON_PARAMS + /** + * Creates a kickoff enemy play FSM + * + * @param ai_config the play config for this play FSM + */ + explicit KickoffEnemyPlayFSM(const TbotsProto::AiConfig& ai_config); + + /** + * create a vector of setup positions if not already existing. + * + * @param world_ptr the world pointer + */ + void createKickoffSetupPositions(const WorldPtr &world_ptr); + + /** + * add shadowing robots to tactics to run. + * + * @param enemy_threats the enemies that must be shadowed. + * @param tactics_to_run vector of tactics to run. + * @param defense_position_index index of robot for priority. + */ + void assignShadowing(const std::vector &enemy_threats, + PriorityTacticVector &tactics_to_run, + size_t &defense_position_index); + + /** + * add defenders to tactics to run. + * + * @param tactics_to_run vector of tactics to run. + * @param defense_position_index index of robot for priority. + */ + void assignDefenders(PriorityTacticVector &tactics_to_run, size_t &defense_position_index); + + /** + * add a goal blocker to tactics to run. + * + * @param world_ptr the world pointer + * @param tactics_to_run vector of tactics to run. + * @param defense_position_index index of robot for priority. + */ + void assignGoalBlocker( + const WorldPtr &world_ptr, + PriorityTacticVector &tactics_to_run, + size_t &defense_position_index + ); + + /** + * Action to organize the bots to be ready for enemy kickoff. + * + * @param event the FreeKickPlayFSM Update event + */ + void kickoff(const Update& event); + + + auto operator()() + { + using namespace boost::sml; + + DEFINE_SML_STATE(SetupState) + + DEFINE_SML_EVENT(Update) + + DEFINE_SML_ACTION(kickoff) + + + return make_transition_table( + // src_state + event [guard] / action = dest_state + // PlaySelectionFSM will transition to OffensePlay after the kick. + *SetupState_S + Update_E / kickoff_A = SetupState_S + ); + } + +private: + TbotsProto::AiConfig ai_config; + std::vector> shadow_enemy_tactics; + std::vector> move_tactics; + std::vector kickoff_setup_positions; +}; \ No newline at end of file diff --git a/src/software/ai/hl/stp/play/kickoff_enemy_play_test.cpp b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.cpp similarity index 97% rename from src/software/ai/hl/stp/play/kickoff_enemy_play_test.cpp rename to src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.cpp index e9ca71ca24..6a363cceaf 100644 --- a/src/software/ai/hl/stp/play/kickoff_enemy_play_test.cpp +++ b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.cpp @@ -1,4 +1,4 @@ -#include "software/ai/hl/stp/play/kickoff_enemy_play.h" +#include "software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play.h" #include diff --git a/src/software/ai/hl/stp/play/kickoff_enemy_play.cpp b/src/software/ai/hl/stp/play/kickoff_enemy_play.cpp deleted file mode 100644 index 694a0c6401..0000000000 --- a/src/software/ai/hl/stp/play/kickoff_enemy_play.cpp +++ /dev/null @@ -1,151 +0,0 @@ -#include "software/ai/hl/stp/play/kickoff_enemy_play.h" - -#include "proto/parameters.pb.h" -#include "shared/constants.h" -#include "software/ai/evaluation/enemy_threat.h" -#include "software/ai/evaluation/possession.h" -#include "software/ai/hl/stp/tactic/move/move_tactic.h" -#include "software/ai/hl/stp/tactic/shadow_enemy/shadow_enemy_tactic.h" -#include "software/geom/algorithms/calculate_block_cone.h" -#include "software/util/generic_factory/generic_factory.h" - -KickoffEnemyPlay::KickoffEnemyPlay(TbotsProto::AiConfig config) : Play(config, true) {} - -void KickoffEnemyPlay::getNextTactics(TacticCoroutine::push_type &yield, - const WorldPtr &world_ptr) -{ - // 3 robots assigned to shadow enemies. Other robots will be assigned positions - // on the field to be evenly spread out - std::vector> shadow_enemy_tactics = { - std::make_shared(), std::make_shared()}; - - // these positions are picked according to the following slide - // https://images.slideplayer.com/32/9922349/slides/slide_2.jpg - // since we only have 6 robots at the maximum, 3 robots will shadow threats - // up front, 1 robot is dedicated as the goalie, and the other 2 robots will defend - // either post (as show in the image) - // - // Positions 1,2 are the most important, 3,4,5 are a fallback - // if there aren't as many threats to shadow. Robots will be assigned - // to those positions in order of priority. The 5 positions shown below - // are in the same order as in the defense_position vector. - // - // +--------------------+--------------------+ - // | | | - // | | | - // | | | - // +--+ 2 4 | +--+ - // | | | | | - // | | +-+-+ | | - // | | 3 | | | | - // | | +-+-+ | | - // | | | | | - // +--+ 1 5 | +--+ - // | | | - // | | | - // | | | - // +--------------------+--------------------+ - - std::vector defense_positions = { - Point(world_ptr->field().friendlyGoalpostNeg().x() + - world_ptr->field().defenseAreaXLength() + 2 * ROBOT_MAX_RADIUS_METERS, - -world_ptr->field().defenseAreaYLength() / 2.0), - Point(world_ptr->field().friendlyGoalpostPos().x() + - world_ptr->field().defenseAreaXLength() + 2 * ROBOT_MAX_RADIUS_METERS, - world_ptr->field().defenseAreaYLength() / 2.0), - Point(world_ptr->field().friendlyGoalCenter().x() + - world_ptr->field().defenseAreaXLength() + 2 * ROBOT_MAX_RADIUS_METERS, - world_ptr->field().friendlyGoalCenter().y()), - Point(-(world_ptr->field().centerCircleRadius() + 2 * ROBOT_MAX_RADIUS_METERS), - world_ptr->field().defenseAreaYLength() / 2.0), - Point(-(world_ptr->field().centerCircleRadius() + 2 * ROBOT_MAX_RADIUS_METERS), - -world_ptr->field().defenseAreaYLength() / 2.0), - }; - // these move tactics will be used to go to those positions - std::vector> move_tactics = { - std::make_shared(), std::make_shared(), - std::make_shared(), std::make_shared(), - std::make_shared()}; - - // created an enemy_team for mutation - Team enemy_team = world_ptr->enemyTeam(); - - do - { - // TODO: (Mathew): Minor instability with defenders and goalie when the ball and - // attacker are in the middle of the net - - // We find the nearest enemy robot closest to (0,0) then ignore it from the enemy - // team. Since the center circle is a motion constraint during enemy kickoff, the - // shadowing robot will navigate to the closest point that it can to shadow, which - // might not be ideal. (i.e robot won't block a straight shot on net) - auto robot = Team::getNearestRobot(world_ptr->enemyTeam().getAllRobots(), - world_ptr->field().centerPoint()); - if (robot.has_value()) - { - int robot_id = robot.value().id(); - enemy_team.removeRobotWithId(robot_id); - } - else - { - LOG(WARNING) << "No Robot on the Field!"; - } - - auto enemy_threats = - getAllEnemyThreats(world_ptr->field(), world_ptr->friendlyTeam(), - world_ptr->enemyTeam(), world_ptr->ball(), false); - - PriorityTacticVector result = {{}}; - - // keeps track of the next defense position to assign - int defense_position_index = 0; - for (unsigned i = 0; i < defense_positions.size() - 1; ++i) - { - if (i < 2 && i < enemy_threats.size()) - { - // Assign the first 2 robots to shadow enemies, if the enemies exist - auto enemy_threat = enemy_threats.at(i); - // Shadow with a distance slightly more than the distance from the enemy - // robot to the center line, so we are always just on our side of the - // center line - double shadow_dist = std::fabs(enemy_threat.robot.position().x()) + - 2 * ROBOT_MAX_RADIUS_METERS; - // We shadow assuming the robots do not pass so we do not try block passes - // while shadowing, since we can't go on the enemy side to block the pass - // anyway - shadow_enemy_tactics.at(i)->updateControlParams(enemy_threat, - shadow_dist); - - result[0].emplace_back(shadow_enemy_tactics.at(i)); - } - else - { - // Once we are out of enemies to shadow, or are already shadowing 2 - // enemies, we move the rest of the robots to the defense positions - // listed above - move_tactics.at(defense_position_index) - ->updateControlParams(defense_positions.at(defense_position_index), - Angle::zero()); - result[0].emplace_back(move_tactics.at(defense_position_index)); - defense_position_index++; - } - } - - // update robot 3 to be directly between the ball and the friendly net - move_tactics.at(defense_position_index) - ->updateControlParams( - calculateBlockCone(world_ptr->field().friendlyGoalpostPos(), - world_ptr->field().friendlyGoalpostNeg(), - world_ptr->field().centerPoint(), - ROBOT_MAX_RADIUS_METERS), - Angle::zero(), TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE); - result[0].emplace_back(move_tactics.at(defense_position_index)); - - // yield the Tactics this Play wants to run, in order of priority - yield(result); - } while (true); -} - -// Register this play in the genericFactory -static TGenericFactory factory; diff --git a/src/software/ai/hl/stp/play/kickoff_enemy_play.h b/src/software/ai/hl/stp/play/kickoff_enemy_play.h deleted file mode 100644 index b6eac6e826..0000000000 --- a/src/software/ai/hl/stp/play/kickoff_enemy_play.h +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include "proto/parameters.pb.h" -#include "software/ai/hl/stp/play/play.h" - -/** - * A play that runs when its currently the enemies kick off, - * prioritizes defending the net and shadowing the robot - * that is nearest to the ball. Any remaining bots will block - * some odd angles to the net. - */ -class KickoffEnemyPlay : public Play -{ - public: - KickoffEnemyPlay(TbotsProto::AiConfig config); - - void getNextTactics(TacticCoroutine::push_type &yield, - const WorldPtr &world_ptr) override; -}; diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/BUILD b/src/software/ai/hl/stp/play/kickoff_friendly/BUILD new file mode 100644 index 0000000000..04de9f6ee0 --- /dev/null +++ b/src/software/ai/hl/stp/play/kickoff_friendly/BUILD @@ -0,0 +1,42 @@ +package(default_visibility = ["//visibility:public"]) + +load("@simulated_tests_deps//:requirements.bzl", "requirement") + +cc_library( + name = "kickoff_friendly_play", + srcs = [ + "kickoff_friendly_play.cpp", + "kickoff_friendly_play_fsm.cpp", + ], + hdrs = [ + "kickoff_friendly_play.h", + "kickoff_friendly_play_fsm.h", + ], + deps = [ + "//software/ai/hl/stp/play", + "//shared:constants", + "//software/ai/evaluation:enemy_threat", + "//software/ai/hl/stp/tactic/chip:chip_tactic", + "//software/ai/hl/stp/tactic/move:move_tactic", + "//software/ai/evaluation:find_open_areas", + "//software/logger", + "//software/util/generic_factory", + ], + alwayslink = True, +) + +cc_test( + name = "kickoff_friendly_play_cpp_test", + srcs = ["kickoff_friendly_play_test.cpp"], + deps = [ + "//shared/test_util:tbots_gtest_main", + "//software/ai/hl/stp/play/kickoff_friendly:kickoff_friendly_play", + "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", + "//software/simulated_tests/non_terminating_validation_functions", + "//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/play/kickoff_friendly/kickoff_friendly_play.cpp b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play.cpp new file mode 100644 index 0000000000..4061d9e700 --- /dev/null +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play.cpp @@ -0,0 +1,35 @@ +#include "software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play.h" + +#include "shared/constants.h" +#include "software/util/generic_factory/generic_factory.h" + + +KickoffFriendlyPlay::KickoffFriendlyPlay(TbotsProto::AiConfig config) + : Play(config, true), fsm{KickoffFriendlyPlayFSM{config}}, control_params{} +{ +} + +void KickoffFriendlyPlay::getNextTactics(TacticCoroutine::push_type &yield, + const WorldPtr &world_ptr) +{ + // Does not get called. + while (true) + { + yield({{}}); + } +} + +void KickoffFriendlyPlay::updateTactics(const PlayUpdate &play_update) +{ + fsm.process_event(KickoffFriendlyPlayFSM::Update(control_params, play_update)); +} + +std::vector KickoffFriendlyPlay::getState() +{ + std::vector state; + state.emplace_back(objectTypeName(*this) + " - " + getCurrentFullStateName(fsm)); + return state; +} + +// Register this play in the genericFactory +static TGenericFactory factory; \ No newline at end of file diff --git a/src/software/ai/hl/stp/play/kickoff_friendly_play.h b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play.h similarity index 50% rename from src/software/ai/hl/stp/play/kickoff_friendly_play.h rename to src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play.h index fda2ea59fe..88cf97f73e 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly_play.h +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play.h @@ -1,17 +1,30 @@ #pragma once #include "proto/parameters.pb.h" +#include "software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.h" #include "software/ai/hl/stp/play/play.h" /** * A play that runs when its currently the friendly kick off, * only one robot grabs the ball and passes to another robot. */ + class KickoffFriendlyPlay : public Play { - public: +public: + /** + * Creates a friendly kickoff play + * + * @param ai_config the play config for this play + */ KickoffFriendlyPlay(TbotsProto::AiConfig config); void getNextTactics(TacticCoroutine::push_type &yield, const WorldPtr &world_ptr) override; -}; + void updateTactics(const PlayUpdate &play_update) override; + std::vector getState() override; + +private: + FSM fsm; + KickoffFriendlyPlayFSM::ControlParams control_params; +}; \ No newline at end of file diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp new file mode 100644 index 0000000000..12eb65ac54 --- /dev/null +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.cpp @@ -0,0 +1,173 @@ +#include "software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.h" + +KickoffFriendlyPlayFSM::KickoffFriendlyPlayFSM(const TbotsProto::AiConfig &ai_config) + : ai_config(ai_config), + kickoff_chip_tactic(std::make_shared()), + shoot_tactic(std::make_shared()), + move_tactics({ + std::make_shared(), // for robot 1 + std::make_shared(), // for robot 2 + std::make_shared(), // for robot 3 + std::make_shared(), // for robot 4 + std::make_shared() // for robot 5 + }) +{ + +} + +void KickoffFriendlyPlayFSM::createKickoffSetupPositions(const WorldPtr &world_ptr) +{ + // Since we only have 6 robots at the maximum, the number one priority + // is the robot doing the kickoff up front. The goalie is the second most + // important, followed by 3 and 4 setup for offense. 5 and 6 will stay + // back near the goalie just in case the ball quickly returns to the friendly + // side of the field. + // + // +--------------------+--------------------+ + // | | | + // | 3 | | + // | | | + // +--+ 5 | +--+ + // | | | | | + // | | +-+-+ | | + // |2 | |1 | | | + // | | +-+-+ | | + // | | | | | + // +--+ 6 | +--+ + // | | | + // | 4 | | + // | | | + // +--------------------+--------------------+ + // + + if (kickoff_setup_positions.empty()) + { + kickoff_setup_positions = { + // Robot 1 + Point(world_ptr->field().centerPoint() + + Vector(-world_ptr->field().centerCircleRadius(), 0)), + // Robot 2 + // Goalie positions will be handled by the goalie tactic + // Robot 3 + Point( + world_ptr->field().centerPoint() + + Vector(-world_ptr->field().centerCircleRadius() - 4 * ROBOT_MAX_RADIUS_METERS, + -1.0 / 3.0 * world_ptr->field().yLength())), + // Robot 4 + Point( + world_ptr->field().centerPoint() + + Vector(-world_ptr->field().centerCircleRadius() - 4 * ROBOT_MAX_RADIUS_METERS, + 1.0 / 3.0 * world_ptr->field().yLength())), + // Robot 5 + Point(world_ptr->field().friendlyGoalpostPos().x() + + world_ptr->field().defenseAreaXLength() + 2 * ROBOT_MAX_RADIUS_METERS, + world_ptr->field().friendlyGoalpostPos().y()), + // Robot 6 + Point(world_ptr->field().friendlyGoalpostNeg().x() + + world_ptr->field().defenseAreaXLength() + 2 * ROBOT_MAX_RADIUS_METERS, + world_ptr->field().friendlyGoalpostNeg().y()), + }; + } +} + + +void KickoffFriendlyPlayFSM::setupKickoff(const Update &event) +{ + createKickoffSetupPositions(event.common.world_ptr); + + PriorityTacticVector tactics_to_run = {{}}; + + // first priority requires the ability to kick and chip. + move_tactics.at(0)->mutableRobotCapabilityRequirements() = { + RobotCapability::Kick, RobotCapability::Chip}; + + // set each tactic to its movement location. + for (unsigned i = 0; i < kickoff_setup_positions.size(); i++) + { + move_tactics.at(i)->updateControlParams(kickoff_setup_positions.at(i), + Angle::zero()); + tactics_to_run[0].emplace_back(move_tactics.at(i)); + } + + event.common.set_tactics(tactics_to_run); +} + +// taken from free kick fsm. +void KickoffFriendlyPlayFSM::shootBall(const Update &event) +{ + WorldPtr world_ptr = event.common.world_ptr; + PriorityTacticVector tactics_to_run = {{}}; + + Point ball_pos = world_ptr->ball().position(); + + shoot_tactic->updateControlParams( + ball_pos, (shot->getPointToShootAt() - ball_pos).orientation(), + BALL_MAX_SPEED_METERS_PER_SECOND); + tactics_to_run[0].emplace_back(shoot_tactic); + + + event.common.set_tactics(tactics_to_run); +} + +void KickoffFriendlyPlayFSM::chipBall(const Update &event) +{ + WorldPtr world_ptr = event.common.world_ptr; + + PriorityTacticVector tactics_to_run = {{}}; + + // adjust with testing to give us enough space to catch the ball before it goes out of bounds + double ballX = world_ptr->ball().position().x(); + double fieldX = world_ptr->field().enemyGoalCenter().x() - 2; + double negFieldY = world_ptr->field().enemyCornerNeg().y() + 0.3; + double posFieldY = world_ptr->field().enemyCornerPos().y() - 0.3; + + Rectangle target_area_rectangle = + Rectangle(Point(ballX, negFieldY), Point(fieldX, posFieldY)); + + // sort targets by distance to enemy goal center. + std::vector potential_chip_targets = findGoodChipTargets(*world_ptr, target_area_rectangle); + std::sort(potential_chip_targets.begin(), potential_chip_targets.end(), + [world_ptr](const Circle& first_circle, const Circle& second_circle) { + + return distance(world_ptr->field().enemyGoalCenter(), first_circle.origin()) < + distance(world_ptr->field().enemyGoalCenter(), second_circle.origin()); + }); + + Point target = world_ptr->field().centerPoint() + Vector(world_ptr->field().xLength() / 6, 0); + + if (!potential_chip_targets.empty()) + { + target = potential_chip_targets[0].origin(); + } + + kickoff_chip_tactic->updateControlParams( + world_ptr->ball().position(), + target + ); + + tactics_to_run[0].emplace_back(kickoff_chip_tactic); + + event.common.set_tactics(tactics_to_run); +} + +bool KickoffFriendlyPlayFSM::isSetupDone(const Update &event) +{ + return !event.common.world_ptr->gameState().isSetupState(); +} + +bool KickoffFriendlyPlayFSM::isPlaying(const Update& event) +{ + return event.common.world_ptr->gameState().isPlaying(); +} + +bool KickoffFriendlyPlayFSM::shotFound(const Update &event) +{ + shot = calcBestShotOnGoal(event.common.world_ptr->field(), + event.common.world_ptr->friendlyTeam(), + event.common.world_ptr->enemyTeam(), + event.common.world_ptr->ball().position(), TeamType::ENEMY); + return shot.has_value() && + shot->getOpenAngle() > + Angle::fromDegrees( + ai_config.attacker_tactic_config().min_open_angle_for_shot_deg()); +} \ No newline at end of file diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.h b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.h new file mode 100644 index 0000000000..7b4dce9cdf --- /dev/null +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_fsm.h @@ -0,0 +1,126 @@ +#pragma once + +#include "proto/parameters.pb.h" +#include "shared/constants.h" +#include "software/ai/evaluation/enemy_threat.h" +#include "software/ai/hl/stp/play/play.h" +#include "software/ai/hl/stp/play/play_fsm.h" +#include "software/ai/hl/stp/tactic/kick/kick_tactic.h" +#include "software/ai/hl/stp/tactic/move/move_tactic.h" +#include "software/ai/hl/stp/tactic/chip/chip_tactic.h" +#include "software/ai/evaluation/find_open_areas.h" +#include "software/logger/logger.h" + +struct KickoffFriendlyPlayFSM +{ + class SetupState; + class ShootState; + class ChipState; + + struct ControlParams + { + }; + + DEFINE_PLAY_UPDATE_STRUCT_WITH_CONTROL_AND_COMMON_PARAMS + /** + * Creates a kickoff friendly play FSM + * + * @param ai_config the play config for this play FSM + */ + explicit KickoffFriendlyPlayFSM(const TbotsProto::AiConfig& ai_config); + + /** + * create a vector of setup positions if not already existing. + * + * @param world_ptr the world pointer + */ + void createKickoffSetupPositions(const WorldPtr &world_ptr); + + + + /** + * Action to move robots to starting positions + * + * @param event the FreeKickPlayFSM Update event + */ + void setupKickoff(const Update& event); + + /** + * Action to shoot the ball at the net. + * + * @param event the FreeKickPlayFSM Update event + */ + void shootBall(const Update& event); + + /** + * Action to chip the ball forward over the defenders. + * + * @param event the FreeKickPlayFSM Update event + */ + void chipBall(const Update& event); + + /** + * Guard that checks if positions are set up. + * + * @param event the FreeKickPlayFSM Update event + */ + bool isSetupDone(const Update& event); + + /** + * Guard that checks if game has started (ball kicked). + * + * @param event the FreeKickPlayFSM Update event + */ + bool isPlaying(const Update& event); + + /** + * Guard that checks if a direct shot on the net is possible. + * + * @param event the FreeKickPlayFSM Update event + */ + bool shotFound(const Update& event); + + auto operator()() + { + using namespace boost::sml; + + DEFINE_SML_STATE(SetupState) + DEFINE_SML_STATE(ShootState) + DEFINE_SML_STATE(ChipState) + + DEFINE_SML_EVENT(Update) + + DEFINE_SML_ACTION(setupKickoff) + DEFINE_SML_ACTION(shootBall) + DEFINE_SML_ACTION(chipBall) + + DEFINE_SML_GUARD(isSetupDone) + DEFINE_SML_GUARD(shotFound) + DEFINE_SML_GUARD(isPlaying) + + return make_transition_table( + // src_state + event [guard] / action = dest_state + // PlaySelectionFSM will transition to OffensePlay after the kick. + *SetupState_S + Update_E[!isSetupDone_G] / setupKickoff_A = SetupState_S, + + // shoot directly at net if possible. + SetupState_S + Update_E[shotFound_G] = ShootState_S, + ShootState_S + Update_E[!isPlaying_G] / shootBall_A = ShootState_S, + ShootState_S + Update_E[isPlaying_G] = X, + + // else chip over the defenders. + SetupState_S + Update_E = ChipState_S, + ChipState_S + Update_E[!isPlaying_G] / chipBall_A = ChipState_S, + ChipState_S + Update_E[isPlaying_G] = X, + + X + Update_E = X); + } + +private: + TbotsProto::AiConfig ai_config; + std::shared_ptr kickoff_chip_tactic; + std::shared_ptr shoot_tactic; + std::vector> move_tactics; + std::vector kickoff_setup_positions; + std::optional shot; +}; \ No newline at end of file diff --git a/src/software/ai/hl/stp/play/kickoff_friendly_play_test.cpp b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.cpp similarity index 97% rename from src/software/ai/hl/stp/play/kickoff_friendly_play_test.cpp rename to src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.cpp index 26b164fcdf..73a5dab569 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly_play_test.cpp +++ b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.cpp @@ -1,4 +1,4 @@ -#include "software/ai/hl/stp/play/kickoff_friendly_play.h" +#include "software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play.h" #include diff --git a/src/software/ai/hl/stp/play/kickoff_friendly_play.cpp b/src/software/ai/hl/stp/play/kickoff_friendly_play.cpp deleted file mode 100644 index bec6ac1cc5..0000000000 --- a/src/software/ai/hl/stp/play/kickoff_friendly_play.cpp +++ /dev/null @@ -1,138 +0,0 @@ -#include "software/ai/hl/stp/play/kickoff_friendly_play.h" - -#include "shared/constants.h" -#include "software/ai/evaluation/enemy_threat.h" -#include "software/ai/hl/stp/tactic/chip/chip_tactic.h" -#include "software/ai/hl/stp/tactic/move/move_tactic.h" -#include "software/util/generic_factory/generic_factory.h" - -KickoffFriendlyPlay::KickoffFriendlyPlay(TbotsProto::AiConfig config) : Play(config, true) -{ -} - -void KickoffFriendlyPlay::getNextTactics(TacticCoroutine::push_type &yield, - const WorldPtr &world_ptr) -{ - // Since we only have 6 robots at the maximum, the number one priority - // is the robot doing the kickoff up front. The goalie is the second most - // important, followed by 3 and 4 setup for offense. 5 and 6 will stay - // back near the goalie just in case the ball quickly returns to the friendly - // side of the field. - // - // +--------------------+--------------------+ - // | | | - // | 3 | | - // | | | - // +--+ 5 | +--+ - // | | | | | - // | | +-+-+ | | - // |2 | |1 | | | - // | | +-+-+ | | - // | | | | | - // +--+ 6 | +--+ - // | | | - // | 4 | | - // | | | - // +--------------------+--------------------+ - // - // This is a two part play: - // Part 1: Get into position, but don't touch the ball (ref kickoff) - // Part 2: Chip the ball over the defender (ref normal start) - - // the following positions are in the same order as the positions shown above, - // excluding the goalie for part 1 of this play - std::vector kickoff_setup_positions = { - // Robot 1 - Point(world_ptr->field().centerPoint() + - Vector(-world_ptr->field().centerCircleRadius(), 0)), - // Robot 2 - // Goalie positions will be handled by the goalie tactic - // Robot 3 - Point( - world_ptr->field().centerPoint() + - Vector(-world_ptr->field().centerCircleRadius() - 4 * ROBOT_MAX_RADIUS_METERS, - -1.0 / 3.0 * world_ptr->field().yLength())), - // Robot 4 - Point( - world_ptr->field().centerPoint() + - Vector(-world_ptr->field().centerCircleRadius() - 4 * ROBOT_MAX_RADIUS_METERS, - 1.0 / 3.0 * world_ptr->field().yLength())), - // Robot 5 - Point(world_ptr->field().friendlyGoalpostPos().x() + - world_ptr->field().defenseAreaXLength() + 2 * ROBOT_MAX_RADIUS_METERS, - world_ptr->field().friendlyGoalpostPos().y()), - // Robot 6 - Point(world_ptr->field().friendlyGoalpostNeg().x() + - world_ptr->field().defenseAreaXLength() + 2 * ROBOT_MAX_RADIUS_METERS, - world_ptr->field().friendlyGoalpostNeg().y()), - }; - - // move tactics to use to move to positions defined above - std::vector> move_tactics = { - std::make_shared(), std::make_shared(), - std::make_shared(), std::make_shared(), - std::make_shared()}; - - // specific tactics - auto kickoff_chip_tactic = std::make_shared(); - - // Part 1: setup state (move to key positions) - while (world_ptr->gameState().isSetupState()) - { - auto enemy_threats = - getAllEnemyThreats(world_ptr->field(), world_ptr->friendlyTeam(), - world_ptr->enemyTeam(), world_ptr->ball(), false); - - PriorityTacticVector result = {{}}; - - // set the requirement that Robot 1 must be able to kick and chip - move_tactics.at(0)->mutableRobotCapabilityRequirements() = { - RobotCapability::Kick, RobotCapability::Chip}; - - // setup 5 kickoff positions in order of priority - for (unsigned i = 0; i < kickoff_setup_positions.size(); i++) - { - move_tactics.at(i)->updateControlParams(kickoff_setup_positions.at(i), - Angle::zero()); - result[0].emplace_back(move_tactics.at(i)); - } - - // yield the Tactics this Play wants to run, in order of priority - yield(result); - } - - // Part 2: not normal play, currently ready state (chip the ball) - while (!world_ptr->gameState().isPlaying()) - { - auto enemy_threats = - getAllEnemyThreats(world_ptr->field(), world_ptr->friendlyTeam(), - world_ptr->enemyTeam(), world_ptr->ball(), false); - - PriorityTacticVector result = {{}}; - - // TODO (#2612): This needs to be adjusted post field testing, ball needs to land - // exactly in the middle of the enemy field - kickoff_chip_tactic->updateControlParams( - world_ptr->ball().position(), - world_ptr->field().centerPoint() + - Vector(world_ptr->field().xLength() / 6, 0)); - result[0].emplace_back(kickoff_chip_tactic); - - // the robot at position 0 will be closest to the ball, so positions starting from - // 1 will be assigned to the rest of the robots - for (unsigned i = 1; i < kickoff_setup_positions.size(); i++) - { - move_tactics.at(i)->updateControlParams(kickoff_setup_positions.at(i), - Angle::zero()); - result[0].emplace_back(move_tactics.at(i)); - } - - // yield the Tactics this Play wants to run, in order of priority - yield(result); - } -} - - -// Register this play in the genericFactory -static TGenericFactory - factory; diff --git a/src/software/ai/hl/stp/play/kickoff_play_test.py b/src/software/ai/hl/stp/play/kickoff_play_test.py index 3675ed40b9..31e3a2fa3c 100644 --- a/src/software/ai/hl/stp/play/kickoff_play_test.py +++ b/src/software/ai/hl/stp/play/kickoff_play_test.py @@ -4,89 +4,141 @@ import software.python_bindings as tbots_cpp from proto.play_pb2 import Play, PlayName +from software.simulated_tests.robot_enters_region import * +from software.simulated_tests.ball_enters_region import * +from software.simulated_tests.ball_moves_from_rest import * from proto.import_all_protos import * from proto.message_translation.tbots_protobuf import create_world_state from proto.ssl_gc_common_pb2 import Team +from software.simulated_tests.or_validation import OrValidation @pytest.mark.parametrize("is_friendly_test", [True, False]) def test_kickoff_play(simulated_test_runner, is_friendly_test): - def setup(*args): - # starting point must be Point - ball_initial_pos = tbots_cpp.Point(0, 0) - - # Setup Bots - blue_bots = [ - tbots_cpp.Point(-3, 2.5), - 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, -2.5), - ] - - yellow_bots = [ - tbots_cpp.Point(1, 0), - tbots_cpp.Point(1, 2.5), - tbots_cpp.Point(1, -2.5), - tbots_cpp.Field.createSSLDivisionBField().enemyGoalCenter(), - tbots_cpp.Field.createSSLDivisionBField() - .enemyDefenseArea() - .negXNegYCorner(), - tbots_cpp.Field.createSSLDivisionBField() - .enemyDefenseArea() - .negXPosYCorner(), - ] - - blue_play = Play() - yellow_play = Play() - - # Game Controller Setup + ball_initial_pos = tbots_cpp.Point(0, 0) + + # Setup Bots + blue_bots = [ + tbots_cpp.Point(-3, 2.5), + 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, -2.5), + ] + + yellow_bots = [ + tbots_cpp.Point(1, 0), + tbots_cpp.Point(1, 2.5), + tbots_cpp.Point(1, -2.5), + tbots_cpp.Field.createSSLDivisionBField().enemyGoalCenter(), + tbots_cpp.Field.createSSLDivisionBField().enemyDefenseArea().negXNegYCorner(), + tbots_cpp.Field.createSSLDivisionBField().enemyDefenseArea().negXPosYCorner(), + ] + + blue_play = Play() + yellow_play = Play() + + # Game Controller Setup + simulated_test_runner.gamecontroller.send_gc_command( + gc_command=Command.Type.STOP, team=Team.UNKNOWN + ) + + if is_friendly_test: simulated_test_runner.gamecontroller.send_gc_command( - gc_command=Command.Type.STOP, team=Team.UNKNOWN + gc_command=Command.Type.KICKOFF, team=Team.BLUE ) + blue_play.name = PlayName.KickoffFriendlyPlay + yellow_play.name = PlayName.KickoffEnemyPlay + else: simulated_test_runner.gamecontroller.send_gc_command( - gc_command=Command.Type.NORMAL_START, team=Team.BLUE + gc_command=Command.Type.KICKOFF, team=Team.YELLOW ) - if is_friendly_test: - simulated_test_runner.gamecontroller.send_gc_command( - gc_command=Command.Type.KICKOFF, team=Team.BLUE - ) - blue_play.name = PlayName.KickoffFriendlyPlay - yellow_play.name = PlayName.KickoffEnemyPlay - else: - simulated_test_runner.gamecontroller.send_gc_command( - gc_command=Command.Type.KICKOFF, team=Team.YELLOW - ) - blue_play.name = PlayName.KickoffEnemyPlay - yellow_play.name = PlayName.KickoffFriendlyPlay + blue_play.name = PlayName.KickoffEnemyPlay + yellow_play.name = PlayName.KickoffFriendlyPlay + + simulated_test_runner.gamecontroller.send_gc_command( + gc_command=Command.Type.NORMAL_START, team=Team.BLUE + ) + + # Force play override here + 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(Play, yellow_play) + + # 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), + ), + ) + + # Always Validation + always_validation_sequence_set = [[]] + + ball_moves_at_rest_validation = BallAlwaysMovesFromRest( + position=tbots_cpp.Point(0, 0), threshold=0.05 + ) + + expected_center_circle_or_validation_set = [ + ball_moves_at_rest_validation, + NumberOfRobotsAlwaysStaysInRegion( + regions=[tbots_cpp.Field.createSSLDivisionBField().centerCircle()], + req_robot_cnt=0, + ), + ] + + friendly_half = tbots_cpp.Field.createSSLDivisionBField().friendlyHalf() + friendly_goal = tbots_cpp.Field.createSSLDivisionBField().friendlyGoal() + center_circle = tbots_cpp.Field.createSSLDivisionBField().centerCircle() - # Force play override here - 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( - Play, yellow_play + friendly_regions = [friendly_half, friendly_goal, center_circle] + + if is_friendly_test: + # this expected_center_circle_or_validation_set version checks + # that either 0 or 1 robots are in centerCircle OR ball moves from center point + expected_center_circle_or_validation_set.append( + NumberOfRobotsAlwaysStaysInRegion( + regions=[tbots_cpp.Field.createSSLDivisionBField().centerCircle()], + req_robot_cnt=1, + ) ) + else: + # Checks that 0 robots are in centerCircle OR ball moves from center point + friendly_regions.remove(center_circle) + + # Checks that there are 6 friendly robots in friendly_regions + # friendly_regions definition depends on if/else case above + expected_robot_regions_or_validations_set = [ + ball_moves_at_rest_validation, + NumberOfRobotsAlwaysStaysInRegion( + regions=friendly_regions, + req_robot_cnt=6, + ), + ] - # 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), - ), + always_validation_sequence_set[0] = [ + OrValidation(expected_center_circle_or_validation_set), + OrValidation(expected_robot_regions_or_validations_set), + ] + + eventually_validation_sequence_set = [[]] + + # Eventually Validation + if is_friendly_test: + # Checks that ball leaves center point by 0.05 meters within 10 seconds of kickoff + eventually_validation_sequence_set[0].append( + BallEventuallyExitsRegion( + regions=[tbots_cpp.Circle(ball_initial_pos, 0.05)] + ) ) - # TODO- #2809 Validation - # params just have to be a list of length 1 to ensure the test runs at least once simulated_test_runner.run_test( - setup=setup, - params=[0], - inv_always_validation_sequence_set=[[]], - inv_eventually_validation_sequence_set=[[]], - ag_always_validation_sequence_set=[[]], - ag_eventually_validation_sequence_set=[[]], + inv_eventually_validation_sequence_set=eventually_validation_sequence_set, + inv_always_validation_sequence_set=always_validation_sequence_set, test_timeout_s=10, ) diff --git a/src/software/ai/play_selection_fsm.cpp b/src/software/ai/play_selection_fsm.cpp index c60e00a7c6..6af378b4db 100644 --- a/src/software/ai/play_selection_fsm.cpp +++ b/src/software/ai/play_selection_fsm.cpp @@ -5,8 +5,8 @@ #include "software/ai/hl/stp/play/enemy_free_kick/enemy_free_kick_play.h" #include "software/ai/hl/stp/play/free_kick/free_kick_play.h" #include "software/ai/hl/stp/play/halt_play/halt_play.h" -#include "software/ai/hl/stp/play/kickoff_enemy_play.h" -#include "software/ai/hl/stp/play/kickoff_friendly_play.h" +#include "software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play.h" +#include "software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play.h" #include "software/ai/hl/stp/play/offense/offense_play.h" #include "software/ai/hl/stp/play/penalty_kick/penalty_kick_play.h" #include "software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play.h" diff --git a/src/software/simulated_tests/ball_enters_region.py b/src/software/simulated_tests/ball_enters_region.py index 6deb272cdd..f7bc00a419 100644 --- a/src/software/simulated_tests/ball_enters_region.py +++ b/src/software/simulated_tests/ball_enters_region.py @@ -33,7 +33,7 @@ def get_validation_status(self, world) -> ValidationStatus: def get_validation_geometry(self, world) -> ValidationGeometry: """Returns the underlying geometry this validation is checking - :param world: The world msg to create v alidation geometry from + :param world: The world msg to create validation geometry from :return: ValidationGeometry containing geometry to visualize """ diff --git a/src/software/simulated_tests/or_validation.py b/src/software/simulated_tests/or_validation.py index 8d7968454f..b59dd1c391 100644 --- a/src/software/simulated_tests/or_validation.py +++ b/src/software/simulated_tests/or_validation.py @@ -6,7 +6,13 @@ class OrValidation(Validation): def __init__(self, validations): - """An or extension to the validation function""" + """An OR extension to the validation function""" + assert len(validations) > 0 + validation_type_initial = validations[0].get_validation_type() + for validation in validations: + validation_type = validation.get_validation_type() + if validation_type != validation_type_initial: + raise TypeError("Type of validation instances is not consistent") self.validations = validations def get_validation_status(self, world): @@ -32,10 +38,7 @@ def get_validation_geometry(self, world): return validation_geometry def get_validation_type(self, world): - validation_type_initial = self.validations[0].get_validation_type + return self.validations[0].get_validation_type(world) - for validation in self.validations: - validation_type = validation.get_validation_type - if validation_type != validation_type_initial: - raise TypeError("type of validation instances is not consistent") - return validation_type_initial + def __repr__(self): + return "PLACEHOLDER" diff --git a/src/software/simulated_tests/robot_enters_region.py b/src/software/simulated_tests/robot_enters_region.py index 672c582bbb..54c5052edc 100644 --- a/src/software/simulated_tests/robot_enters_region.py +++ b/src/software/simulated_tests/robot_enters_region.py @@ -8,92 +8,52 @@ ) -class RobotEntersRegion(Validation): - """Checks if a Robot enters any of the provided regions.""" +class MinNumberOfRobotsEntersRegion(Validation): + """Checks if a certain number of Robots enters a specific set of regions.""" - def __init__(self, regions=None): - self.regions = regions if regions else [] - self.passing_robot = None + def __init__(self, regions, req_robot_cnt): + """Initializes the validation class with a set of regions and required count of robots + + :param regions: the regions that will be checked for robot count + :param req_robot_cnt: the minimum number of unique robots that must be in the given regions + """ + self.regions = regions + self.req_robot_cnt = req_robot_cnt + # map to keep track of robot positions + self.robot_in_zone = {} def get_validation_status(self, world) -> ValidationStatus: - """Checks if _any_ robot enters the provided regions + """Checks if a specific number of robots enter the provided set of regions :param world: The world msg to validate - :return: FAILING until a robot enters any of the regions - PASSING when a robot enters + :returns: FAILING until req_robot_cnt robots enter the set of regions + PASSING when req_robot_cnt robots enter the set of regions + # """ + robots_in_regions = set() for region in self.regions: for robot in world.friendly_team.team_robots: if tbots_cpp.contains( region, tbots_cpp.createPoint(robot.current_state.global_position) ): - self.passing_robot = robot - return ValidationStatus.PASSING - - self.passing_robot = None - return ValidationStatus.FAILING - - def get_validation_geometry(self, world) -> ValidationGeometry: - """(override) shows regions to enter""" - return create_validation_geometry(self.regions) - - def __repr__(self): - return "Check for robot in regions " + ",".join( - repr(region) for region in self.regions - ) - - -( - RobotEventuallyEntersRegion, - RobotEventuallyExitsRegion, - RobotAlwaysStaysInRegion, - RobotNeverEntersRegion, -) = create_validation_types(RobotEntersRegion) + robots_in_regions.add(robot.id) - -class NumberOfRobotsEntersRegion(Validation): - """Checks if a certain number of Robots enters a specific region.""" - - def __init__(self, region, req_robot_cnt): - self.region = region - self.req_robot_cnt = req_robot_cnt - # map to keep track of robot positions - self.robot_in_zone = {} - - def get_validation_status(self, world) -> ValidationStatus: - """Checks if a specific number of robots enter the provided region - - :param world: The world msg to validate - :return: FAILING until req_robot_cnt robots enter the region - PASSING when req_robot_cnt robots enters - """ - # Update the map with latest robot status - for robot in world.friendly_team.team_robots: - self.robot_in_zone[robot.id] = tbots_cpp.contains( - self.region, tbots_cpp.createPoint(robot.current_state.global_position) - ) - # Check if there are at least req_robot_cnt number of robots in zone - curr_cnt = 0 - for robot_id in self.robot_in_zone: - if self.robot_in_zone[robot_id]: - curr_cnt += 1 - - # Validate on curr_cnt - if curr_cnt == self.req_robot_cnt: + # Validate on length of set robots_in_regions + if len(robots_in_regions) >= self.req_robot_cnt: return ValidationStatus.PASSING - else: - return ValidationStatus.FAILING + + return ValidationStatus.FAILING def get_validation_geometry(self, world) -> ValidationGeometry: """(override) shows region to enter""" - return create_validation_geometry([self.region]) + return create_validation_geometry(self.regions) def __repr__(self): return ( "Check for " + str(self.req_robot_cnt) + " robots in region " - + ",".join(repr(self.region)) + + ",".join(repr(self.regions)) ) @@ -102,4 +62,23 @@ def __repr__(self): NumberOfRobotsEventuallyExitsRegion, NumberOfRobotsAlwaysStaysInRegion, NumberOfRobotsNeverEntersRegion, -) = create_validation_types(NumberOfRobotsEntersRegion) +) = create_validation_types(MinNumberOfRobotsEntersRegion) + + +class RobotEntersRegion(MinNumberOfRobotsEntersRegion): + """Checks if at least one robot is contained within the given regions""" + + def __init__(self, regions): + """Initializes the validation class with a set of regions + + :param regions: the regions that will be checked to contain at least one robot + """ + super(RobotEntersRegion, self).__init__(regions, 1) + + +( + RobotEventuallyEntersRegion, + RobotEventuallyExitsRegion, + RobotAlwaysStaysInRegion, + RobotNeverEntersRegion, +) = create_validation_types(RobotEntersRegion)