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..acf1b89f66 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,173 @@ 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.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 = [[]] + if is_friendly_test: + # Checks that either 0 or 1 robots are in centerCircle OR ball moves from center point + always_validation_sequence_set[0].append( + OrValidation( + [ + BallAlwaysMovesFromRest( + position=tbots_cpp.Point(0, 0), threshold=0.01 + ), + NumberOfRobotsAlwaysStaysInRegion( + regions=[ + tbots_cpp.Field.createSSLDivisionBField().centerCircle() + ], + req_robot_cnt=0, + ), + NumberOfRobotsAlwaysStaysInRegion( + regions=[ + tbots_cpp.Field.createSSLDivisionBField().centerCircle() + ], + req_robot_cnt=1, + ), + ] ) - 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 + ) + + # Checks that there are 6 friendly robots in friendlyHalf + centerCircle + friendlyGoal OR ball moves from center point + always_validation_sequence_set[0].append( + OrValidation( + [ + BallAlwaysMovesFromRest( + position=tbots_cpp.Point(0, 0), threshold=0.01 + ), + NumberOfRobotsAlwaysStaysInRegion( + regions=[ + tbots_cpp.Field.createSSLDivisionBField().friendlyHalf(), + tbots_cpp.Field.createSSLDivisionBField().friendlyGoal(), + tbots_cpp.Field.createSSLDivisionBField().centerCircle(), + ], + req_robot_cnt=6, + ), + ] + ) + ) + + else: + # Checks that 0 robots are in centerCircle OR ball moves from center point + always_validation_sequence_set[0].append( + OrValidation( + [ + BallAlwaysMovesFromRest( + position=tbots_cpp.Point(0, 0), threshold=0.01 + ), + NumberOfRobotsAlwaysStaysInRegion( + regions=[ + tbots_cpp.Field.createSSLDivisionBField().centerCircle() + ], + req_robot_cnt=0, + ), + ] ) - blue_play.name = PlayName.KickoffEnemyPlay - yellow_play.name = PlayName.KickoffFriendlyPlay + ) - # 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 + # Checks that there are 6 enemy robots in friendlyHalf + centerCircle + friendlyGoal OR ball moves from center point + always_validation_sequence_set[0].append( + OrValidation( + [ + BallAlwaysMovesFromRest( + position=tbots_cpp.Point(0, 0), threshold=0.01 + ), + NumberOfRobotsAlwaysStaysInRegion( + regions=[ + tbots_cpp.Field.createSSLDivisionBField().friendlyHalf(), + tbots_cpp.Field.createSSLDivisionBField().friendlyGoal(), + ], + 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), - ), + 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/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..cc716dfba1 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,4 @@ def get_validation_geometry(self, world): return validation_geometry def get_validation_type(self, world): - validation_type_initial = self.validations[0].get_validation_type - - 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 + return ValidationType 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)