def __init__(self, ball: Ball): # <-------------------------field_length------------------------> # # ^ +------------------------------+-------------------------------+ # | | their side | our side | # | | | | # | +-----------+ | Y+ E----------+ # f| | | | ^ | | # i| | | | | | | # e| +--+ | | +--> X+ | C--+ ^ # l| | | | | | | | | # d| | | | XX|XX | | | | # | | | | X | X | | | | # w| | B--------------------------------------------------------------A | | goal_width # i| | | | X | X | | | | # d| | | | XX|XX | | | | # t| | | | | | | | | # h| +--+ | | | D--+ v # | | | | | | # | | | | | | # | +-----------+ | H----------F # | | | | # | | | | # v +------------------------------+-------------------------------+ # self.logger = logging.getLogger(self.__class__.__name__) self._ball = ball self.our_goal = None # Point A self.our_goal_pose = None self.their_goal = None # Point B self.their_goal_pose = None self.our_goal_line = None self.their_goal_line = None self.goal_line = None # Point C to D self.our_goal_area = None # Area define by Point E to F self.their_goal_area = None # Default values, used only for UT self.field_length = 4500 self.field_width = 3000 self.goal_width = 1000 self.goal_depth = 200 self.center_circle_radius = 1000 self.boundary_width = 300 # Is the distance between the field and the outside wall self.field_lines = { "RightPenaltyStretch": Line( p1=Position(3495, -1000), # H to E p2=Position(3495, +1000)), "RightFieldLeftPenaltyStretch": Line( p1=Position(4490, -1000), # H to F p2=Position(3490, -1000)) } self.field_arcs = {} self._update_field_const()
def debug_cmd(self): if not self.is_debug: return angle = None additional_dbg = [] if self.current_state == self.go_behind_ball: angle = 18 elif self.current_state == self.grab_ball: angle = 25 elif self.current_state == self.kick: angle = 45 additional_dbg = [DebugCommandFactory.circle(self.game_state.ball_position, KICK_DISTANCE, color=RED)] if angle is not None: angle *= np.pi/180.0 base_angle = (self.game_state.ball.position - self.target.position).angle magnitude = 3000 ori = self.game_state.ball.position upper = ori + Position.from_angle(base_angle + angle, magnitude) lower = ori + Position.from_angle(base_angle - angle, magnitude) ball_to_player = self.player.position - self.game_state.ball_position behind_player = (ball_to_player.norm + 1000) * normalize(ball_to_player) + self.game_state.ball_position return [DebugCommandFactory.line(ori, upper), DebugCommandFactory.line(ori, lower), DebugCommandFactory.line(self.game_state.ball_position, behind_player, color=CYAN)] + additional_dbg return []
def next_position(self): self.target_orientation = (self.target.position - self.game_state.ball_position).angle self.position = (self.game_state.ball_position - Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL) if self.start_time is not None: if time.time() - self.start_time >= self.rotate_time: self.rotation_sign = self._get_direction() if compare_angle(self.target_orientation, (self.game_state.ball_position - self.player.position).angle, VALID_DIFF_ANGLE): self.next_state = self.halt return self._go_to_final_position() elif time.time() - self.iter_time >= self.switch_time: self.iter_time = time.time() self._switch_rotation() if (self.player.pose.position - self.position).norm < VALID_DISTANCE: if self.start_time is None: self.start_time = time.time() self.iter_time = time.time() self.ball_collision = True self.speed = 1 self.offset_orientation += DIFF_ANGLE * self.rotation_sign self.position = (self.game_state.ball_position - Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL) if self.start_time is not None: orientation = self.offset_orientation if time.time() - self.start_time < \ self.rotate_time else self.target_orientation else: orientation = self.target_orientation return CmdBuilder().addMoveTo(Pose(self.position, orientation), cruise_speed=self.speed, ball_collision=self.ball_collision).build()
def stay_inside_square(position, y_top, y_bottom, x_left, x_right): # Parameters assertions assert isinstance(position, Position) assert isinstance(y_top, (int, float)) assert isinstance(y_bottom, (int, float)) assert isinstance(x_left, (int, float)) assert isinstance(x_right, (int, float)) assert y_top > y_bottom assert x_right > x_left if is_inside_square(position, y_top, y_bottom, x_left, x_right): return Position(position.x, position.y) else: pos_x = position.x pos_y = position.y if pos_y > y_top: pos_y = y_top elif pos_y < y_bottom: pos_y = y_bottom if pos_x > x_right: pos_x = x_right elif pos_x < x_left: pos_x = x_left return Position(pos_x, pos_y)
def stay_inside_circle(position, center, radius): # Parameters assertions if is_inside_circle(position, center, radius): return Position(position.x, position.y) pos_angle = (position - center).angle pos_x = radius * m.cos(pos_angle) + center.x pos_y = radius * m.sin(pos_angle) + center.y return Position(pos_x, pos_y)
def _line(start: Position, end: Position, color=MAGENTA, timeout=DEFAULT_DEBUG_TIMEOUT): return debug_command( 3001, { 'start': start.to_tuple(), 'end': end.to_tuple(), 'color': color.repr(), 'timeout': timeout })
def _fix_ulaval_field_line(self, field): # The penalty x y is point E in the sketch penalty_x = self.field_lines["RightPenaltyStretch"].p1.x penalty_y = self.field_arcs["RightFieldRightPenaltyArc"].center.y + \ self.field_arcs["RightFieldRightPenaltyArc"].radius self.field_lines["RightPenaltyStretch"] \ = Line(p1=Position(penalty_x, -penalty_y), p2=Position(penalty_x, +penalty_y)) goal_x = field["field_length"] / 2 self.field_lines["RightFieldLeftPenaltyStretch"] \ = Line(p1=Position(goal_x, -penalty_y), p2=Position(penalty_x, -penalty_y))
def _change_tactic(self, cmd: STAChangeCommand): try: this_player = GameState().our_team.available_players[cmd.data['id']] except KeyError: self.logger.info('A tactic was assign to a player which is not on the field (id={}).'.format(cmd.data['id'])) this_player = GameState().our_team.players[cmd.data['id']] player_id = this_player.id tactic_name = cmd.data['tactic'] target = Position.from_list(cmd.data['target']) if config['GAME']['on_negative_side']: target = target.flip_x() target = Pose(target, this_player.orientation) args = cmd.data.get('args', '') try: tactic = self.play_state.get_new_tactic(tactic_name)(self.game_state, this_player, target, args) except: self.logger.exception('An error occurred.') self.logger.debug('The tactic was call with wrong arguments') raise if not isinstance(self.play_state.current_strategy, HumanControl): self.play_state.current_strategy = 'HumanControl' self.play_state.current_strategy.assign_tactic(tactic, player_id)
def _format_entities(entities: Iterable[Union[RobotFilter, BallFilter]]) -> List[Dict[str, Any]]: formatted_list = [] for entity in entities: fields = dict() if type(entity) is RobotFilter: fields['pose'] = Pose.from_values(*entity.pose) fields['velocity'] = Pose.from_values(*entity.velocity) elif type(entity) is BallFilter: fields['position'] = Position(*entity.position) fields['velocity'] = Position(*entity.velocity) else: raise TypeError('Invalid type provided: {}'.format(type(entity))) fields['id'] = entity.id formatted_list.append(fields) return formatted_list
def _go_to_final_position(self): position = self.game_state.ball_position - Position.from_angle( self.target_orientation) * DISTANCE_FROM_BALL return CmdBuilder().addMoveTo( Pose(position, self.target_orientation), cruise_speed=self.speed, ball_collision=self.ball_collision).build()
def compute_wall_arc(self): nb_robots = len(self.robots_in_formation) # Angle d'un robot sur le cercle de rayon self.dist_from_ball self.robot_angle = 2 * atan(ROBOT_RADIUS / self.dist_from_ball) # Angle totale couvert par le wall self.wall_angle = nb_robots * self.robot_angle + GAP_ANGLE * ( nb_robots - 1) goal_line = self.game_state.field.goal_line self.center_of_goal = find_bisector_of_triangle( self.game_state.ball_position, goal_line.p1, goal_line.p2) vec_ball_to_center_of_goal = self.center_of_goal - self.game_state.ball_position # Centre de la formation sur le cercle de rayon self.dist_from_ball self.center_formation = self.dist_from_ball * normalize( vec_ball_to_center_of_goal) + self.game_state.ball_position # J'ajoute robot_angle / 2, sinon, avec 1 seul robot, il ne se positionne pas dans le centre de la formation self.first_robot_angle = vec_ball_to_center_of_goal.angle - self.wall_angle / 2 + self.robot_angle / 2 self.first_robot_position = Position(cos(self.first_robot_angle), sin(self.first_robot_angle)) * self.dist_from_ball + \ self.game_state.ball_position self.last_robot_angle = self.first_robot_angle + self.wall_angle
def __init__( self, game_state: GameState, player: Player, target: Pose = Pose(), penalty_kick=False, args: List[str] = None, ): forbidden_area = [ Area.pad(game_state.field.their_goal_area, KEEPOUT_DISTANCE_FROM_GOAL) ] super().__init__(game_state, player, target, args, forbidden_areas=forbidden_area) self.current_state = self.defense self.next_state = self.defense self.target = Pose( self.game_state.field.our_goal, np.pi) # Ignore target argument, always go for our goal self.go_kick_tactic = None # Used by clear self.last_intersection = None # For debug self.OFFSET_FROM_GOAL_LINE = Position(ROBOT_RADIUS + 10, 0) self.GOAL_LINE = self.game_state.field.our_goal_line
def __init__(self, game_state: GameState, player: Player, target: Pose = Pose(), args: List[str] = None, p_y_top: [int, float] = 3000, p_y_bottom: [int, float] = -3000, p_x_left: [int, float] = -4500, p_x_right: [int, float] = 4500, p_is_yellow: bool = False): super().__init__(game_state, player, target, args) assert isinstance(p_y_top, (int, float)) assert isinstance(p_y_bottom, (int, float)) assert isinstance(p_x_left, (int, float)) assert isinstance(p_x_right, (int, float)) assert isinstance(p_is_yellow, bool) self.y_top = p_y_top self.y_bottom = p_y_bottom self.x_left = p_x_left self.x_right = p_x_right self.is_yellow = p_is_yellow self.current_state = self.cover_zone self.next_state = self.cover_zone self.status_flag = Flags.WIP self.target = Pose( Position(int((p_x_right - p_x_left) / 2), int((p_y_top - p_y_bottom) / 2)))
def __init__(self, game_state: GameState): super().__init__(game_state) self.target_position = Position( self.game_state.field.our_goal_area.left - 3 * ROBOT_RADIUS, 0) number_of_player = len(self.game_state.our_team.available_players) role_to_positions = {} for i, role in enumerate(Role.as_list()): position_offset = Position(0, (i - (number_of_player - 1) / 2) * ROBOT_RADIUS * 4) role_to_positions[role] = Pose(self.target_position + position_offset) self.assign_tactics(role_to_positions)
def flip_position(position: Position): """ The AI is side independent, so every position send to the UI-Debug must be flip around the y axis """ assert isinstance(position, Position) if not config["GAME"]["on_negative_side"]: return position return position.flip_x()
def stay_outside_square(position, y_top, y_bottom, x_left, x_right): # Parameters assertions assert isinstance(position, Position) assert isinstance(y_top, (int, float)) assert isinstance(y_bottom, (int, float)) assert isinstance(x_left, (int, float)) assert isinstance(x_right, (int, float)) assert y_top > y_bottom assert x_right > x_left if is_outside_square(position, y_top, y_bottom, x_left, x_right): return Position(position.x, position.y) pos_y = y_top if position.y > y_top - (y_top - y_bottom) / 2 else y_bottom pos_x = x_right if position.x > x_right - (x_right - x_left) / 2 else x_left return Position(pos_x, pos_y)
def main_state(self): self.compute_wall_arc() if self.game_state.field.is_ball_in_our_goal_area(): return Idle # We must not block the goalkeeper angle = self.angle_on_wall_arc() dest = Position(cos(angle), sin( angle)) * self.dist_from_ball + self.game_state.ball_position dest_orientation = (self.game_state.ball_position - dest).angle return MoveTo(Pose(dest, dest_orientation), cruise_speed=0.8)
def __init__(self, game_state: GameState, player: Player, target: Pose = Pose, args: List[str] = None, cruise_speed=DEFAULT_SPEED): super().__init__(game_state, player, target, args) self.current_state = self.next_corner self.next_state = self.next_corner self.cruise_speed = cruise_speed self.iteration = 0 self.x_sign = 1 self.y_sign = 1 self.coord_x = game_state.field.field_length / 3 self.coord_y = game_state.field.field_width / 3 self.points = [ WayPoint(Position(self.coord_x, self.coord_y)), WayPoint(Position(-self.coord_x, self.coord_y)), WayPoint(Position(-self.coord_x, -self.coord_y)), WayPoint(Position(self.coord_x, -self.coord_y)) ] self.points = deque(self.points)
def _circle(center: Position, radius, color=LIGHT_GREEN, is_fill=True, timeout=DEFAULT_DEBUG_TIMEOUT): return debug_command( 3003, { 'center': center.to_tuple(), 'radius': radius, 'color': color.repr(), 'is_fill': is_fill, 'timeout': timeout })
def next_position(self): self.target_orientation = (self.target.position - self.game_state.ball_position).angle self.position = ( self.game_state.ball_position - Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL) if self.start_time is not None: if time.time() - self.start_time >= self.rotate_time: self.rotation_sign = self._get_direction() if compare_angle(self.target_orientation, (self.game_state.ball_position - self.player.position).angle, VALID_DIFF_ANGLE): self.next_state = self.halt return self._go_to_final_position() elif time.time() - self.iter_time >= self.switch_time: self.iter_time = time.time() self._switch_rotation() if (self.player.pose.position - self.position).norm < VALID_DISTANCE: if self.start_time is None: self.start_time = time.time() self.iter_time = time.time() self.ball_collision = True self.speed = 1 self.offset_orientation += DIFF_ANGLE * self.rotation_sign self.position = (self.game_state.ball_position - Position.from_angle(self.offset_orientation) * DISTANCE_FROM_BALL) if self.start_time is not None: orientation = self.offset_orientation if time.time() - self.start_time < \ self.rotate_time else self.target_orientation else: orientation = self.target_orientation return CmdBuilder().addMoveTo( Pose(self.position, orientation), cruise_speed=self.speed, ball_collision=self.ball_collision).build()
def _update_field_const(self): self.our_goal = Position(self.our_goal_x, 0) self.our_goal_pose = Pose(self.our_goal, 0) self.their_goal = Position(self.their_goal_x, 0) self.their_goal_pose = Pose(self.their_goal, 0) p1 = self.field_lines["RightPenaltyStretch"].p1 p2 = self.field_lines["RightPenaltyStretch"].p2 p3 = self.field_lines["RightFieldLeftPenaltyStretch"].p1 p4 = self.field_lines["RightFieldLeftPenaltyStretch"].p2 self.our_goal_area = Area.from_4_point(p1, p2, p3, p4) self.their_goal_area = Area.flip_x(self.our_goal_area) self.goal_line = Line(p1=Position(self.our_goal_x, +self.goal_width / 2), p2=Position(self.our_goal_x, -self.goal_width / 2)) self.our_goal_line = Line(p1=Position(self.our_goal_x, +self.goal_width / 2), p2=Position(self.our_goal_x, -self.goal_width / 2)) self.their_goal_line = Line(p1=Position(self.their_goal_x, +self.goal_width / 2), p2=Position(self.their_goal_x, -self.goal_width / 2)) self.behind_our_goal_line = Area.from_limits( self.our_goal_area.top, self.our_goal_area.bottom, self.our_goal_area.right + 50 * self.goal_depth, self.our_goal_area.left) self.behind_their_goal_line = Area.flip_x(self.behind_our_goal_line) self.free_kick_avoid_area = Area.pad( self.their_goal_area, INDIRECT_KICK_OFFSET + KEEPOUT_DISTANCE_FROM_GOAL) self.our_goal_forbidden_area = Area.pad(self.our_goal_area, KEEPOUT_DISTANCE_FROM_GOAL) self.their_goal_forbidden_area = Area.pad(self.their_goal_area, KEEPOUT_DISTANCE_FROM_GOAL) self.center = Position(0, 0)
def __init__(self, game_state: GameState, player: Player, args: List[str]=None, center_of_zone=Position(0, 0), height_of_zone=800, width_of_zone=800): super().__init__(game_state, player, args=args) self.current_state = self.main_state self.next_state = self.main_state self.center_of_zone = center_of_zone self.height_of_zone = height_of_zone self.width_of_zone = width_of_zone self.bottom_left_corner = Position(self.center_of_zone[0] - self.width_of_zone / 2, self.center_of_zone[1] - self.height_of_zone / 2) self.grid_of_positions = [] discretisation = 100 for i in range(int(self.width_of_zone / discretisation)): for j in range(int(self.height_of_zone / discretisation)): self.grid_of_positions.append(self.bottom_left_corner + Position(discretisation * i, discretisation * j)) self.current_position_index_to_go = random.randint(0, len(self.grid_of_positions) - 1) self.current_position_to_go = self.grid_of_positions[self.current_position_index_to_go] self.current_angle_to_go = 0 #random.randint(0, 100) * np.pi / 100. self.next_pose = Pose(self.current_position_to_go, self.current_angle_to_go)
def push_ball(self): # self.debug.add_log(1, "Grab ball called") # self.debug.add_log(1, "vector player 2 ball : {} mm".format(self.vector_norm)) if (self.last_ball_position - self.player.pose.position).norm < 40: self.next_state = self.halt self.last_time = time.time() elif self._is_player_opposing_ball_and_target(-0.9): self.next_state = self.push_ball else: self.next_state = self.get_behind_ball # self.debug.add_log(1, "orientation go get ball {}".format(self.last_angle)) target = self.target.position player = self.player.pose.position player_to_target = target - player player_to_target = 0.5 * player_to_target / np.linalg.norm(player_to_target) speed_pose = Pose(Position.from_array(player_to_target)) return MoveTo(speed_pose)
def robot(pose: Pose, color=LIGHT_GREEN, color_angle=RED, radius=120, timeout=0.05): pose = flip_pose(pose) cmd = [ DebugCommandFactory._circle(pose.position, radius, color=color, timeout=timeout) ] start = pose.position end = start + Position(radius * cos(pose.orientation), radius * sin(pose.orientation)) cmd += [DebugCommandFactory._line(start, end, color_angle, timeout)] return cmd
def push_ball(self): # self.debug.add_log(1, "Grab ball called") # self.debug.add_log(1, "vector player 2 ball : {} mm".format(self.vector_norm)) if (self.last_ball_position - self.player.pose.position).norm < 40: self.next_state = self.halt self.last_time = time.time() elif self._is_player_opposing_ball_and_target(-0.9): self.next_state = self.push_ball else: self.next_state = self.get_behind_ball # self.debug.add_log(1, "orientation go get ball {}".format(self.last_angle)) target = self.target.position player = self.player.pose.position player_to_target = target - player player_to_target = 0.5 * player_to_target / np.linalg.norm( player_to_target) speed_pose = Pose(Position.from_array(player_to_target)) return MoveTo(speed_pose)
def cover_zone(self): enemy_positions = self.get_enemy_in_zone() ball_pos = self.game_state.ball_position if len(enemy_positions) == 0: self.next_state = self.support_other_zone return Idle else: self.next_state = self.cover_zone mean_position = Position() for pos in enemy_positions: mean_position = mean_position + pos mean_position /= len(enemy_positions) destination = stay_inside_square(mean_position, self.y_top, self.y_bottom, self.x_left, self.x_right) return GoBetween(self.game_state, self.player, ball_pos, destination, ball_pos, 2 * ROBOT_RADIUS)
def test_initialize_tactic(tactic_class): simulator = PerfectSim(tactic_class) mock_id = 1 mock_pose = Pose(Position(3, 5)) try: simulator.start(mock_id, mock_pose) except AssertionError: pass # This is done to pass snowflake tactic that require additional arguments # TODO: Fix tactics so they pass that test # def test_few_ticks_tactic(tactic_class): # FEW_TICKS = 10 # simulator = PerfectSim(tactic_class) # mock_id = 1 # mock_pose = Pose(Position(3, 5)) # simulator.start(mock_id, mock_pose) # # for _ in range(0, FEW_TICKS): # simulator.tick()
def _find_best_player_position(self): if not self.auto_position: return self.target.position if self.is_offense: ball_offset = clamp(self.game_state.ball.position.x, 0, 1000) left = self.game_state.field.their_goal_area.right + ball_offset right = ball_offset else: ball_offset = clamp(self.game_state.ball.position.x, -1000, 0) left = self.game_state.field.our_goal_area.left + ball_offset right = ball_offset idx = self.idx_in_formation len_formation = len(self.robots_in_formation) PADDING = 300 # Add buffer zone between area and the field limit area_height = self.game_state.field.field_width - 2 * PADDING individual_area_size = area_height / len_formation top = idx * individual_area_size - area_height / 2 bot = top + individual_area_size self.area = Area.from_limits(top, bot, right, left) center = self.area.center # Act as if each enemy robot was creating a repulsive force v = Position(0, 0) for enemy in self.game_state.enemy_team.available_players.values(): d = enemy.position - center # Clamp distance norm d = MIN_DIST_FROM_CENTER * normalize(d) if d.norm < MIN_DIST_FROM_CENTER else d # Square of the inverse of the distance, a bit like Newton's law of universal gravitation v -= ATTENUATION * d / (d.norm ** 3) if self.area.point_inside(center + v): return center + v offset_from_center = Line(center, center + v) # The position must stay inside the area limits, so let's find the intersection between our vector and the area area_inter = self.area.intersect(offset_from_center) if area_inter: return area_inter[0] # First intersection return center + v
def _best_target_into_goal(self): if 0 < len(self.game_state.enemy_team.available_players): enemy_player_with_ball = player_with_ball(min_dist_from_ball=200, our_team=False) if enemy_player_with_ball is not None: if player_pointing_toward_segment(enemy_player_with_ball, self.GOAL_LINE): ball = self.game_state.ball where_ball_enter_goal = intersection_between_lines( self.GOAL_LINE.p1, self.GOAL_LINE.p2, ball.position, ball.position + Position( 1000 * np.cos(enemy_player_with_ball.pose.orientation), 1000 * np.sin(enemy_player_with_ball.pose.orientation))) where_ball_enter_goal = closest_point_on_segment( where_ball_enter_goal, self.GOAL_LINE.p1, self.GOAL_LINE.p2) return where_ball_enter_goal return find_bisector_of_triangle(self.game_state.ball.position, self.GOAL_LINE.p2, self.GOAL_LINE.p1)
def ProtectGoal(game_state: GameState, player: Player, is_right_goal: bool = True, minimum_distance: float = 150 / 2, maximum_distance: float = None): """ Calcul la pose que doit prendre le gardien en fonction de la position de la balle. :return: Un tuple (Pose, kick) où Pose est la destination du gardien et kick est nul (on ne botte pas) """ goalkeeper_position = player.pose.position ball_position = game_state.ball_position goal_x = game_state.field.our_goal_x goal_position = Position(goal_x, 0) # Calcul des deux positions extremums entre la balle et le centre du but inner_circle_position = stay_inside_circle(ball_position, goal_position, minimum_distance) outer_circle_position = stay_inside_circle(ball_position, goal_position, maximum_distance) destination_position = closest_point_on_segment(goalkeeper_position, inner_circle_position, outer_circle_position) # Vérification que destination_position respecte la distance maximale if maximum_distance is None: destination_position = game_state.game.field.stay_inside_goal_area( destination_position, our_goal=True) else: destination_position = stay_inside_circle(destination_position, goal_position, maximum_distance) # Calcul de l'orientation de la pose de destination destination_orientation = (ball_position - destination_position).angle destination_pose = Pose(destination_position, destination_orientation) return MoveTo(destination_pose)
def _go_to_final_position(self): position = self.game_state.ball_position - Position.from_angle(self.target_orientation) * DISTANCE_FROM_BALL return CmdBuilder().addMoveTo(Pose(position, self.target_orientation), cruise_speed=self.speed, ball_collision=self.ball_collision).build()
def __init__(self, arc: Dict): self.center = Position.from_dict(arc["center"]) self.radius = arc["radius"] self.angle_start = arc["a1"] # Counter clockwise order self.angle_end = arc["a2"] self.thickness = arc["thickness"]
def __init__(self, line: Dict): self.p1 = Position.from_dict(line["p1"]) self.p2 = Position.from_dict(line["p2"]) self.thickness = line["thickness"]
def test_givenDict_whenFromDict_thenPositionIsInstantiated(self): pos = Position.from_dict(A_DICT) self.assertEqual(pos.x, A_DICT['x']) self.assertEqual(pos.y, A_DICT['y'])
def test_givenList_whenFromList_thenPositionIsInstantiated(self): pos = Position.from_list(A_LIST) self.assertEqual(pos.x, A_LIST[0]) self.assertEqual(pos.y, A_LIST[1])
def test_givenNumpyArray_whenFromArray_thenPositionIsCopy(self): pos = Position.from_array(A_ARRAY) self.assertIsNot(pos, A_ARRAY)
def test_givenNumpyArray_whenFromArray_thenReturnNewPosition(self): pos = Position.from_array(A_ARRAY) self.assertEqual(pos.x, A_ARRAY[0]) self.assertEqual(pos.y, A_ARRAY[1])
def _line(start: Position, end: Position, color=MAGENTA, timeout=DEFAULT_DEBUG_TIMEOUT): return debug_command(3001, {'start': start.to_tuple(), 'end': end.to_tuple(), 'color': color.repr(), 'timeout': timeout})
def _circle(center: Position, radius, color=LIGHT_GREEN, is_fill=True, timeout=DEFAULT_DEBUG_TIMEOUT): return debug_command(3003, {'center': center.to_tuple(), 'radius': radius, 'color': color.repr(), 'is_fill': is_fill, 'timeout': timeout})