diff --git a/docs/fsm-diagrams.md b/docs/fsm-diagrams.md index 8059b8e10d..555ad57164 100644 --- a/docs/fsm-diagrams.md +++ b/docs/fsm-diagrams.md @@ -25,6 +25,18 @@ Terminate:::terminate --> Terminate:::terminate ``` +## [StopPlayFSM](/src/software/ai/hl/stp/play/stop_play_fsm.h) + +```mermaid + +stateDiagram-v2 +classDef terminate fill:white,color:black,font-weight:bold +direction LR +[*] --> StopState +StopState --> StopState : updateStopPosition + +``` + ## [BallPlacementPlayFSM](/src/software/ai/hl/stp/play/ball_placement/ball_placement_play_fsm.h) ```mermaid diff --git a/src/software/ai/hl/stp/play/BUILD b/src/software/ai/hl/stp/play/BUILD index db672cbc01..2ef29d5e59 100644 --- a/src/software/ai/hl/stp/play/BUILD +++ b/src/software/ai/hl/stp/play/BUILD @@ -66,16 +66,20 @@ cc_library( cc_library( name = "stop_play", - srcs = ["stop_play.cpp"], - hdrs = ["stop_play.h"], + srcs = [ + "stop_play.cpp", + "stop_play_fsm.cpp", + ], + hdrs = [ + "stop_play.h", + "stop_play_fsm.h", + ], deps = [ ":play", "//shared:constants", - "//software/ai/evaluation:enemy_threat", "//software/ai/hl/stp/tactic/crease_defender:crease_defender_tactic", "//software/ai/hl/stp/tactic/goalie:goalie_tactic", "//software/ai/hl/stp/tactic/move:move_tactic", - "//software/logger", "//software/util/generic_factory", ], alwayslink = True, @@ -175,7 +179,7 @@ py_test( ) cc_test( - name = "stop_play_test", + name = "stop_play_cpp_test", srcs = ["stop_play_test.cpp"], deps = [ "//shared/test_util:tbots_gtest_main", @@ -189,6 +193,25 @@ cc_test( ], ) +py_test( + name = "stop_play_test", + srcs = [ + "stop_play_test.py", + ], + # The default main would be stop_play_test.py; override to our file. + main = "stop_play_test.py", + # TODO (#2619) Remove tag to run in parallel + tags = [ + "exclusive", + ], + deps = [ + "//software:conftest", + "//software/simulated_tests:speed_threshold_helpers", + "//software/simulated_tests:validation", + requirement("pytest"), + ], +) + cc_test( name = "shoot_or_chip_play_cpp_test", srcs = ["shoot_or_chip_play_test.cpp"], diff --git a/src/software/ai/hl/stp/play/stop_play.cpp b/src/software/ai/hl/stp/play/stop_play.cpp index f865221ae1..7bd27bc54f 100644 --- a/src/software/ai/hl/stp/play/stop_play.cpp +++ b/src/software/ai/hl/stp/play/stop_play.cpp @@ -1,116 +1,23 @@ #include "software/ai/hl/stp/play/stop_play.h" -#include "shared/constants.h" -#include "software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic.h" #include "software/ai/hl/stp/tactic/goalie/goalie_tactic.h" -#include "software/ai/hl/stp/tactic/move/move_tactic.h" #include "software/util/generic_factory/generic_factory.h" StopPlay::StopPlay(std::shared_ptr ai_config_ptr) - : Play(ai_config_ptr, true) + : PlayBase(ai_config_ptr, true) { + goalie_tactic = std::make_shared(ai_config_ptr); + goalie_tactic->updateMaxSpeedMode(TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND); } void StopPlay::getNextTactics(TacticCoroutine::push_type &yield, const WorldPtr &world_ptr) { - // Robot assignments for the Stop Play - // - 1 robot will be the goalie - // - 2 robots will assist the goalie in blocking the ball, they will snap - // to the best fit semicircle around the defense area - // - 3 robots will stay within 0.5m of the ball, evenly spaced, also blocking the - // goal - // - // If x represents the ball and G represents the goalie, the following, also blocking - // the goal diagram depicts a possible outcome of this play - // - // +--------------------+--------------------+ - // | | | - // | 4 x | | - // | 0 2 | | - // +--+ 1 3 | +--+ - // | | | | | - // |G | +-+-+ | | - // | | | | | | - // | | +-+-+ | | - // | | | | | - // +--+ | +--+ - // | | | - // | | | - // | | | - // +--------------------+--------------------+ - - - TbotsProto::MaxAllowedSpeedMode stop_mode = - TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND; - - std::vector> move_tactics = { - std::make_shared(ai_config_ptr), - std::make_shared(ai_config_ptr), - std::make_shared(ai_config_ptr)}; - - goalie_tactic = std::make_shared(ai_config_ptr); - goalie_tactic->updateMaxSpeedMode(stop_mode); - std::array, 2> crease_defender_tactics = { - std::make_shared(ai_config_ptr), - std::make_shared(ai_config_ptr), - }; - - do - { - PriorityTacticVector result = {{}}; - - // a unit vector from the center of the goal to the ball, this vector will be used - // for positioning all the robots (excluding the goalie). The positioning vector - // will be used to position robots tangent to the goal_to_ball_unit_vector - Vector goal_to_ball_unit_vector = - (world_ptr->field().friendlyGoalCenter() - world_ptr->ball().position()) - .normalize(); - Vector robot_positioning_unit_vector = goal_to_ball_unit_vector.perpendicular(); - - // ball_defense_point_center is a point on the circle around the ball that the - // line from the center of the goal to the ball intersects. A robot will be placed - // on that line, and the other two will be on either side - // We add an extra robot radius as a buffer to be extra safe we don't break any - // rules by getting too close - Point ball_defense_point_center = - world_ptr->ball().position() + - (0.5 + 2 * ROBOT_MAX_RADIUS_METERS) * goal_to_ball_unit_vector; - Point ball_defense_point_left = - ball_defense_point_center - - robot_positioning_unit_vector * 4 * ROBOT_MAX_RADIUS_METERS; - Point ball_defense_point_right = - ball_defense_point_center + - robot_positioning_unit_vector * 4 * ROBOT_MAX_RADIUS_METERS; - - move_tactics.at(0)->updateControlParams( - ball_defense_point_center, - (world_ptr->ball().position() - ball_defense_point_center).orientation(), - stop_mode, TbotsProto::ObstacleAvoidanceMode::SAFE); - move_tactics.at(1)->updateControlParams( - ball_defense_point_left, - (world_ptr->ball().position() - ball_defense_point_left).orientation(), - stop_mode, TbotsProto::ObstacleAvoidanceMode::SAFE); - move_tactics.at(2)->updateControlParams( - ball_defense_point_right, - (world_ptr->ball().position() - ball_defense_point_right).orientation(), - stop_mode, TbotsProto::ObstacleAvoidanceMode::SAFE); - - std::get<0>(crease_defender_tactics) - ->updateControlParams(world_ptr->ball().position(), - TbotsProto::CreaseDefenderAlignment::LEFT, stop_mode, - TbotsProto::BallStealMode::IGNORE); - std::get<1>(crease_defender_tactics) - ->updateControlParams(world_ptr->ball().position(), - TbotsProto::CreaseDefenderAlignment::RIGHT, stop_mode, - TbotsProto::BallStealMode::IGNORE); +} - // insert all the tactics to the result - result[0].emplace_back(std::get<0>(crease_defender_tactics)); - result[0].emplace_back(std::get<1>(crease_defender_tactics)); - result[0].insert(result[0].end(), move_tactics.begin(), move_tactics.end()); - yield(result); - } while (true); +void StopPlay::updateTactics(const PlayUpdate &play_update) +{ + fsm.process_event(StopPlayFSM::Update(control_params, play_update)); } // Register this play in the genericFactory diff --git a/src/software/ai/hl/stp/play/stop_play.h b/src/software/ai/hl/stp/play/stop_play.h index 511a77925b..a572918d1f 100644 --- a/src/software/ai/hl/stp/play/stop_play.h +++ b/src/software/ai/hl/stp/play/stop_play.h @@ -2,17 +2,20 @@ #include "proto/parameters.pb.h" #include "software/ai/hl/stp/play/play.h" +#include "software/ai/hl/stp/play/play_base.hpp" +#include "software/ai/hl/stp/play/stop_play_fsm.h" /** * This Play moves our robots in a formation while keeping them at least 0.5m from the * ball. Additionally, the robots are limited to moving no more than 1.5m/s. This Play is * used during the referee "Stop" command. */ -class StopPlay : public Play +class StopPlay : public PlayBase { public: StopPlay(std::shared_ptr ai_config_ptr); void getNextTactics(TacticCoroutine::push_type &yield, const WorldPtr &world_ptr) override; + void updateTactics(const PlayUpdate &play_update) override; }; diff --git a/src/software/ai/hl/stp/play/stop_play_fsm.cpp b/src/software/ai/hl/stp/play/stop_play_fsm.cpp new file mode 100644 index 0000000000..53bfccb253 --- /dev/null +++ b/src/software/ai/hl/stp/play/stop_play_fsm.cpp @@ -0,0 +1,104 @@ +#include "software/ai/hl/stp/play/stop_play_fsm.h" + +#include "shared/constants.h" + +StopPlayFSM::StopPlayFSM(std::shared_ptr ai_config_ptr) + : ai_config_ptr(ai_config_ptr), + move_tactics{std::make_shared(ai_config_ptr), + std::make_shared(ai_config_ptr), + std::make_shared(ai_config_ptr)}, + crease_defender_tactics{std::make_shared(ai_config_ptr), + std::make_shared(ai_config_ptr)} +{ +} + +void StopPlayFSM::updateStopPosition(const Update& event) +{ + // Robot assignments for the Stop Play + // - 1 robot will be the goalie + // - 2 robots will assist the goalie in blocking the ball, they will snap + // to the best fit semicircle around the defense area + // - 2 robots will stay within 0.5m of the ball, curved around the goal-to-ball + // line + // - 1 robot will position more centrally in our half to help stretch the field + // + // If x represents the ball and G represents the goalie, the following diagram + // depicts a possible outcome of this play + // + // +--------------------+--------------------+ + // | | | + // | 4 x | | + // | 0 2 | | + // +--+ 1 | +--+ + // | | | | | + // |G | +-+-+ | | + // | | | | | | + // | | +-+-+ | | + // | | 3 | | | + // +--+ | +--+ + // | | | + // | | | + // | | | + // +--------------------+--------------------+ + const WorldPtr& world_ptr = event.common.world_ptr; + TbotsProto::MaxAllowedSpeedMode stop_mode = + TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND; + + // A unit vector from the center of the goal to the ball; used for + // positioning all non-goalie robots. The perpendicular is used to place + // robots tangent to the goal-to-ball line. + Vector goal_to_ball_unit_vector = + (world_ptr->field().friendlyGoalCenter() - world_ptr->ball().position()) + .normalize(); + Vector robot_positioning_unit_vector = goal_to_ball_unit_vector.perpendicular(); + + // Points on the circle around the ball: center on the goal-ball line, + // and one offset. Extra robot radius buffer to stay within rules. + Point ball_defense_point_center = + world_ptr->ball().position() + + (0.5 + 2 * ROBOT_MAX_RADIUS_METERS) * goal_to_ball_unit_vector; + Point ball_defense_point_right = + ball_defense_point_center + + robot_positioning_unit_vector * 4 * ROBOT_MAX_RADIUS_METERS; + + // A spread-out position for the support robot: we take the point along the + // goal-to-ball line at CENTRAL_SUPPORT_FRACTION, then mirror it across the + // field center so the support robot is on the opposite side of the field + // from the ball (stretching the field). + constexpr double CENTRAL_SUPPORT_FRACTION = 0.5; + Point point_along_goal_ball = + world_ptr->field().friendlyGoalCenter() + + (world_ptr->ball().position() - world_ptr->field().friendlyGoalCenter()) * + CENTRAL_SUPPORT_FRACTION; + + Point field_center = world_ptr->field().centerPoint(); + Point central_support_point = field_center + (field_center - point_along_goal_ball); + + move_tactics.at(0)->updateControlParams( + ball_defense_point_center, + (world_ptr->ball().position() - ball_defense_point_center).orientation(), + stop_mode, TbotsProto::ObstacleAvoidanceMode::SAFE); + move_tactics.at(1)->updateControlParams( + ball_defense_point_right, + (world_ptr->ball().position() - ball_defense_point_right).orientation(), + stop_mode, TbotsProto::ObstacleAvoidanceMode::SAFE); + move_tactics.at(2)->updateControlParams( + central_support_point, + (world_ptr->ball().position() - central_support_point).orientation(), stop_mode, + TbotsProto::ObstacleAvoidanceMode::SAFE); + + std::get<0>(crease_defender_tactics) + ->updateControlParams(world_ptr->ball().position(), + TbotsProto::CreaseDefenderAlignment::LEFT, stop_mode, + TbotsProto::BallStealMode::IGNORE); + std::get<1>(crease_defender_tactics) + ->updateControlParams(world_ptr->ball().position(), + TbotsProto::CreaseDefenderAlignment::RIGHT, stop_mode, + TbotsProto::BallStealMode::IGNORE); + + PriorityTacticVector result = {{}}; + result[0].emplace_back(std::get<0>(crease_defender_tactics)); + result[0].emplace_back(std::get<1>(crease_defender_tactics)); + result[0].insert(result[0].end(), move_tactics.begin(), move_tactics.end()); + event.common.set_tactics(result); +} diff --git a/src/software/ai/hl/stp/play/stop_play_fsm.h b/src/software/ai/hl/stp/play/stop_play_fsm.h new file mode 100644 index 0000000000..6d21b7bbc0 --- /dev/null +++ b/src/software/ai/hl/stp/play/stop_play_fsm.h @@ -0,0 +1,62 @@ +#pragma once + +#include + +#include "proto/parameters.pb.h" +#include "shared/constants.h" +#include "software/ai/hl/stp/play/play_fsm.hpp" +#include "software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic.h" +#include "software/ai/hl/stp/tactic/move/move_tactic.h" + +struct StopPlayFSM +{ + struct ControlParams + { + }; + class StopState; + + struct Update + { + Update(const ControlParams& control_params, const PlayUpdate& common) + : control_params(control_params), common(common) + { + } + ControlParams control_params; + PlayUpdate common; + }; + + /** + * Creates a Stop Play FSM + * + * @param ai_config_ptr shared pointer to the play config for this FSM + */ + explicit StopPlayFSM(std::shared_ptr ai_config_ptr); + + /** + * Action to position robots during Stop (goalie handled by Play; this sets + * crease defenders and robots near the ball). + * + * @param event the StopPlayFSM Update event + */ + void updateStopPosition(const Update& event); + + auto operator()() + { + using namespace boost::sml; + + DEFINE_SML_STATE(StopState) + + DEFINE_SML_EVENT(Update) + + DEFINE_SML_ACTION(updateStopPosition) + + return make_transition_table( + // src_state + event [guard] / action = dest_state + *StopState_S + Update_E / updateStopPosition_A = StopState_S); + } + + private: + std::shared_ptr ai_config_ptr; + std::vector> move_tactics; + std::array, 2> crease_defender_tactics; +}; diff --git a/src/software/ai/hl/stp/play/stop_play_test.py b/src/software/ai/hl/stp/play/stop_play_test.py new file mode 100644 index 0000000000..c2ba47b362 --- /dev/null +++ b/src/software/ai/hl/stp/play/stop_play_test.py @@ -0,0 +1,130 @@ +import software.python_bindings as tbots_cpp +from proto.import_all_protos import * +from proto.play_pb2 import Play, PlayName +from proto.message_translation.tbots_protobuf import create_world_state +from proto.ssl_gc_common_pb2 import Team +from software.py_constants import ROBOT_MAX_RADIUS_METERS +from software.simulated_tests.robot_speed_threshold import ( + RobotSpeedEventuallyBelowThreshold, +) +from software.simulated_tests.simulated_test_fixture import ( + pytest_main, +) +from software.simulated_tests.validation import ( + Validation, + create_validation_geometry, +) +from software.simulated_tests.validation import ValidationStatus, ValidationType +from typing import override + + +class FriendlyRobotPositionsVisualization(Validation): + """Always-passing validation that draws circles at each friendly robot position. + Add to inv_always_validation_sequence_set and run with --enable_thunderscope to + visualize robot positions during the test. + """ + + @override + def get_validation_status(self, world) -> ValidationStatus: + return ValidationStatus.PASSING + + @override + def get_validation_type(self, world) -> ValidationType: + return ValidationType.ALWAYS + + @override + def get_validation_geometry(self, world) -> ValidationGeometry: + return create_validation_geometry( + [ + tbots_cpp.Circle( + tbots_cpp.Point( + robot.current_state.global_position.x_meters, + robot.current_state.global_position.y_meters, + ), + ROBOT_MAX_RADIUS_METERS + 0.05, + ) + for robot in world.friendly_team.team_robots + ] + ) + + def __repr__(self): + return "Friendly robot positions (visualization only)" + + +def test_stop_play(simulated_test_runner): + """Test stop play: robots slow down and avoid the ball (STOP referee state).""" + + def setup(*args): + # Ball at centre (matches C++ test_stop_play_ball_at_centre_robots_spread_out) + ball_initial_pos = tbots_cpp.Point(0, 0) + + field = tbots_cpp.Field.createSSLDivisionBField() + + blue_bots = [ + tbots_cpp.Point(-4, 0), + tbots_cpp.Point(-0.3, 0), + tbots_cpp.Point(0.3, 0), + tbots_cpp.Point(0, 0.3), + tbots_cpp.Point(-3, -1.5), + tbots_cpp.Point(4.6, -3.1), + ] + + yellow_bots = [ + tbots_cpp.Point(1, 0), + tbots_cpp.Point(1, 2.5), + tbots_cpp.Point(1, -2.5), + field.enemyGoalCenter(), + field.enemyDefenseArea().negXNegYCorner(), + field.enemyDefenseArea().negXPosYCorner(), + ] + + # Game controller: STOP (both teams) - stop play behaviour + 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.UNKNOWN + ) + + # Force play override: blue runs StopPlay, yellow runs HaltPlay + blue_play = Play() + blue_play.name = PlayName.StopPlay + + yellow_play = Play() + yellow_play.name = PlayName.HaltPlay + + 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 + ) + + 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), + ), + ) + + # C++ test waits 8s before checking; use 15s timeout so robots have time to slow. + # Threshold 1.4 m/s: expect robots to eventually slow below the 1.5 m/s STOP limit. + # TODO: add an eventually-validation that friendly robots stay at least 0.5 m away + # from the ball once pytest fixtures support delaying validations (to mirror the + # C++ robotsAvoidBall(0.5, ...) check). + simulated_test_runner.run_test( + setup=setup, + params=[0], + inv_always_validation_sequence_set=[ + [FriendlyRobotPositionsVisualization()], + ], + inv_eventually_validation_sequence_set=[ + [RobotSpeedEventuallyBelowThreshold(1.4)] + ], + test_timeout_s=15, + ) + + +if __name__ == "__main__": + pytest_main(__file__)