Merge pull request #240 from RoboCupULaval/maintenance_STA_evaluation…

…_module

Maintenance sta evaluation module

RULEngine/Communication/sender/serial_command_sender.py

@@ -11,7 +11,7 @@
 from RULEngine.Command.command import *
 
 COMMUNICATION_SLEEP = 0.001
-MOVE_COMMAND_SLEEP = 0.05
+MOVE_COMMAND_SLEEP = 0.01
 
 
 class SerialCommandSender(object):

RULEngine/Communication/sender/uidebug_robot_monitor.py

@@ -28,9 +28,9 @@ def __init__(self, serial_com: SerialCommandSender, debug_interface: DebugInterf
         self.robots_status = {}
         for robot_id in range(PLAYER_PER_TEAM):
             self.robots_status[robot_id] = RobotStatus()
-        self.terminate = threading.Event()
-        self.monitor_thread = threading.Thread(target=self._monitor_loop)
-        self.pause_cond = threading.Condition(threading.Lock())
+        # self.terminate = threading.Event()
+        # self.monitor_thread = threading.Thread(target=self._monitor_loop)
+        # self.pause_cond = threading.Condition(threading.Lock())
 
         # self.monitor_thread.start()
 
@@ -56,6 +56,7 @@ def _monitor_loop(self):
         return
 
     def stop(self):
-        self.terminate.set()
-        self.monitor_thread.join()
-        self.terminate.clear()
+        pass
+        # self.terminate.set()
+        # self.monitor_thread.join()
+        # self.terminate.clear()

RULEngine/Debug/debug_interface.py

@@ -28,6 +28,7 @@ def repr(self):
 BLUE = Color(38, 139, 210)
 CYAN = Color(42, 161, 152)
 GREEN = Color(133, 153, 0)
+BLACK = Color(0, 0, 0)
 
 # Alias pour les identifiants des robots
 COLOR_ID0 = YELLOW
@@ -36,13 +37,15 @@ def repr(self):
 COLOR_ID3 = MAGENTA
 COLOR_ID4 = VIOLET
 COLOR_ID5 = BLUE
+COLOR_ID6 = BLACK
 
 COLOR_ID_MAP = {0: COLOR_ID0,
                 1: COLOR_ID1,
                 2: COLOR_ID2,
                 3: COLOR_ID3,
                 4: COLOR_ID4,
-                5: COLOR_ID5}
+                5: COLOR_ID5,
+                6: COLOR_ID6}
 
 DEFAULT_TEXT_SIZE = 14  # px
 DEFAULT_TEXT_FONT = 'Arial'
@@ -92,18 +95,18 @@ def add_multiple_points(self, points, color=VIOLET, width=5, link=None, timeout=
         self.debug_state.append(point)
 
     def add_circle(self, center, radius, color=CYAN.repr(), is_fill=True, timeout=DEFAULT_DEBUG_TIMEOUT):
-        data = {'center': center,
+        data = {'center': (int(center[0]), int(center[1])),
                 'radius': radius,
                 'color': color,
                 'is_fill': is_fill,
                 'timeout': timeout}
         circle = DebugCommand(3003, data)
         self.debug_state.append(circle)
 
-    def add_line(self, start_point, end_point, timeout=DEFAULT_DEBUG_TIMEOUT):
+    def add_line(self, start_point, end_point, timeout=DEFAULT_DEBUG_TIMEOUT, color=MAGENTA.repr()):
         data = {'start': start_point,
                 'end': end_point,
-                'color': MAGENTA.repr(),
+                'color': color,
                 'timeout': timeout}
         command = DebugCommand(3001, data)
         self.debug_state.append(command)

RULEngine/Game/Field.py

@@ -176,7 +176,8 @@ def update_field_dimensions(self, packets):
             self._goal_width = field.goal_width
             self._goal_depth = field.goal_depth
             self._center_circle_radius = self.field_arcs['CenterCircle'].radius
-            self._defense_stretch = self.field_lines['LeftPenaltyStretch'].length
+            self._defense_stretch = 100 # hard coded parce que cette valeur d'est plus valide et que plusieurs modules en ont de besoin
+            #la valeur qu'on avait apres le fix a Babin était de 9295 mm, ce qui est 90 fois la grandeur d'avant.
 
             self.constant["FIELD_Y_TOP"] = self._field_width / 2
             self.constant["FIELD_Y_BOTTOM"] = -self._field_width / 2

RULEngine/Game/OurPlayer.py

@@ -7,10 +7,10 @@
 
 
 class OurPlayer(Player):
-    max_speed = 2
-    max_angular_speed = 3.14
-    max_acc = 1.5
-    max_angular_acc = 0.3
+    max_speed = 3
+    max_angular_speed = 2
+    max_acc = 2
+    max_angular_acc = 1
 
     def __init__(self, team, id: int):
         super().__init__(team=team, id=id)

RULEngine/Util/constant.py

@@ -25,7 +25,7 @@
 RADIUS_TO_HALT = ROBOT_RADIUS + BALL_RADIUS
 
 # Deadzones
-POSITION_DEADZONE = ROBOT_RADIUS * 0.5
+POSITION_DEADZONE = ROBOT_RADIUS * 0.1
 
 # Orientation abs_tol
 ORIENTATION_ABSOLUTE_TOLERANCE = 0.01  # 0.5 degree

RULEngine/Util/kalman_filter/ball_kalman_filter.py

@@ -113,5 +113,6 @@ def filter(self, observation=None, dt=0.05):
                 self.x[3] = 0
         self.predict()
         output_state = self.x
+        #print("pos", self.x[0], self.x[1])
         #print("speed", self.x[2], self.x[3])
         return output_state

RULEngine/Util/kalman_filter/friend_kalman_filter.py

@@ -63,13 +63,21 @@ def predict(self, command):
             self.x = np.dot(self.F, self.x)
         else:
             conversion_m_to_mm = 1000
-            command = Pose(*command).scale(conversion_m_to_mm)
+            command = Pose(command[0], command[1], command[2]).scale(conversion_m_to_mm)
             command.position = command.position.rotate(self.x[4])
-            self.x = np.dot(self.F, self.x) + np.dot(self.B, command.to_array())
+            oldx = self.x.copy()
+            self.x = np.dot(self.F, oldx) + np.dot(self.B, command.to_array())
+
         self.P = np.dot(np.dot(self.F, self.P), np.transpose(self.F)) + self.Q
         if self.P[0][0] is np.nan:
             exit(0)
 
+        if np.abs(self.x[5]) > 500:
+            print("A The estimate of orientation speed is reaching inf!!")
+            self.x[5] = 0
+            self.P[5][5] = 0
+
+
     # @profile(immediate=False)
     def update(self, observation):
 
@@ -106,6 +114,10 @@ def update(self, observation):
             self.x = self.x + np.dot(K, np.transpose(y))
             self.P = np.dot((self.eye - np.dot(K, H)), self.P)
 
+        if np.abs(self.x[5]) > 500:
+            print("B The estimate of orientation speed is reaching inf!!")
+            self.x[5] = 0
+
     def transition_model_with_command(self, dt):
         self.F = np.array([[1, 0, dt, 0, 0, 0],  # Position x
                            [0, 1, 0, dt, 0, 0],  # Position y

ai/Algorithm/evaluation_module.py

@@ -20,28 +20,29 @@ def player_with_ball(min_dist_from_ball=1.2*ROBOT_RADIUS, our_team=None):
         return None
 
 
-def closest_players_to_point(point: Position, our_team=None):
+def closest_players_to_point(point: Position, our_team=None, robots=None):
     # Retourne une liste de tuples (player, distance) en ordre croissant de distance,
     # our_team pour obtenir une liste contenant une équipe en particulier
-    list_player = []
-    if our_team or our_team is None:
-        for i in GameState().my_team.available_players.values():
+    if robots is None:
+        if our_team or our_team is None:
             # les players friends
-            player_distance = (i.pose.position - point).norm()
-            list_player.append(PlayerPosition(i, player_distance))
-    if not our_team:
-        for i in GameState().other_team.available_players.values():
+            robots = GameState().my_team.available_players.values()
+        else:
             # les players ennemis
-            player_distance = (i.pose.position - point).norm()
-            list_player.append(PlayerPosition(i, player_distance))
+            robots = GameState().other_team.available_players.values()
+    list_player = []
+
+    for i in robots:
+        player_distance = (i.pose.position - point).norm()
+        list_player.append(PlayerPosition(i, player_distance))
     list_player = sorted(list_player, key=lambda x: x.distance)
     return list_player
 
 
-def closest_player_to_point(point: Position, our_team=None):
+def closest_player_to_point(point: Position, our_team=None, robots=None):
     # Retourne le player le plus proche,
     # our_team pour obtenir une liste contenant une équipe en particulier
-    return closest_players_to_point(point, our_team)[0]
+    return closest_players_to_point(point, our_team, robots)[0]
 
 
 def is_ball_moving(min_speed=0.1):
@@ -251,8 +252,11 @@ def best_position_in_region(player, A, B):
 
     saturation_modifier = np.clip((positions[:, 0] - x_closest_to_our_side) / width, 0.05, 1)
     scores /= saturation_modifier
-    best_score_index = np.argmin(scores)
-    best_position = positions[best_score_index, :]
+    try:
+        best_score_index = np.argmin(scores)
+        best_position = positions[best_score_index, :]
+    except:
+        best_position = Position()
 
     return best_position
 

ai/Algorithm/path_partitionner.py

@@ -150,7 +150,7 @@ def get_path(self, player: CollisionBody, pose_target: CollisionBody, cruise_spe
         # Debug code pls no remove
         # if old_path is not None:
         self.get_pertinent_collision_objects()
-        self.path = Path(self.player.position, self.pose_target.position, 0, self.end_speed * 1000)
+        self.path = Path(self.player.position, self.pose_target.position, self.player.velocity.norm(), self.end_speed * 1000)
         if len(self.collision_body) <= 0:
             return self.path, self.path
         #if old_raw_path is not None:

ai/STA/Action/ProtectGoal.py

@@ -7,6 +7,7 @@
 from RULEngine.Util.area import stayInsideCircle
 from RULEngine.Util.geometry import get_closest_point_on_segment
 from ai.STA.Action.Action import Action
+from ai.STA.Tactic.go_to_position_pathfinder import GoToPositionPathfinder
 from ai.Util.ai_command import AICommand, AICommandType
 from ai.states.game_state import GameState
 
@@ -70,11 +71,12 @@ def exec(self):
             destination_position = stayInsideCircle(destination_position, goal_position, self.maximum_distance)
 
         # Calcul de l'orientation de la pose de destination
-        destination_orientation = (ball_position - destination_position).angle()
+        #destination_orientation = (ball_position - destination_position).angle()
 
-        destination_pose = Pose(destination_position, destination_orientation)
-        return AICommand(self.player,
-                         AICommandType.MOVE,
-                         pose_goal=destination_pose,
-                         pathfinder_on=False)
+        destination_pose = Pose(destination_position, self.player.pose.orientation)
+        # return AICommand(self.player,
+        #                  AICommandType.MOVE,
+        #                  pose_goal=destination_pose,
+        #                  pathfinder_on=False)
+        return GoToPositionPathfinder(self.game_state, self.player, destination_pose, end_speed=0.5).exec()
 

ai/STA/Strategy/defense_wall.py

@@ -31,14 +31,10 @@ def __init__(self, game_state: GameState, number_of_players: int = 4):
             if player:
                 self.add_tactic(role, AlignToDefenseWall(self.game_state, player, self.robots))
                 self.add_tactic(role, GoKick(self.game_state, player, target=self.theirgoal))
-                self.add_tactic(role, FaceOpponent(self.game_state, player, Pose()))
 
                 self.add_condition(role, 0, 1, partial(self.is_closest, player))
-                self.add_condition(role, 2, 1, partial(self.is_closest, player))
                 self.add_condition(role, 1, 1, partial(self.is_closest, player))
-                self.add_condition(role, 1, 2, partial(self.is_not_closest, player))
-                self.add_condition(role, 2, 0, partial(self.is_not_one_of_the_closests, player))
-                self.add_condition(role, 0, 2, partial(self.is_second_closest, player))
+                self.add_condition(role, 1, 0, partial(self.is_not_closest, player))
 
     def is_closest(self, player):
         if player == closest_players_to_point(GameState().get_ball_position(), True)[0].player:

ai/STA/Strategy/defense_wall_3v3.py

@@ -0,0 +1,64 @@
+# Under MIT license, see LICENSE.txt
+from functools import partial
+
+from ai.Algorithm.evaluation_module import closest_players_to_point, Pose, Position
+from ai.STA.Tactic.align_to_defense_wall import AlignToDefenseWall
+from ai.STA.Tactic.goalkeeper import GoalKeeper
+from ai.STA.Tactic.face_opponent import FaceOpponent
+from ai.STA.Tactic.go_kick import GoKick
+from ai.STA.Tactic.tactic_constants import Flags
+from ai.Util.role import Role
+from ai.states.game_state import GameState
+from ai.STA.Strategy.strategy import Strategy
+
+class DefenseWall_3v3(Strategy):
+    def __init__(self, game_state: GameState, number_of_players: int = 3):
+        super().__init__(game_state)
+
+        role_mapping = {Role.GOALKEEPER: 4, Role.FIRST_ATTACK: 3, Role.SECOND_ATTACK: 2}
+        self.game_state.map_players_to_roles_by_player_id(role_mapping)
+
+        self.number_of_players = number_of_players
+        self.robots = []
+        ourgoal = Pose(Position(GameState().const["FIELD_OUR_GOAL_X_EXTERNAL"], 0), 0)
+        self.theirgoal = Pose(Position(GameState().const["FIELD_THEIR_GOAL_X_EXTERNAL"], 0), 0)
+
+        roles_to_consider = [Role.FIRST_ATTACK, Role.SECOND_ATTACK]
+
+        goalkeeper = self.game_state.get_player_by_role(Role.GOALKEEPER)
+        self.add_tactic(Role.GOALKEEPER, GoalKeeper(self.game_state, goalkeeper, target=ourgoal))
+
+        role_by_robots = [(i, self.game_state.get_player_by_role(i)) for i in roles_to_consider]
+        self.robots = [player for _, player in role_by_robots if player is not None]
+        for role, player in role_by_robots:
+            if player:
+                self.add_tactic(role, AlignToDefenseWall(self.game_state, player, self.robots))
+                self.add_tactic(role, GoKick(self.game_state, player, target=self.theirgoal))
+
+                self.add_condition(role, 0, 1, partial(self.is_closest, player))
+                self.add_condition(role, 1, 1, partial(self.is_closest, player))
+                self.add_condition(role, 1, 0, partial(self.is_not_closest, player))
+
+    def is_closest(self, player):
+        if player == closest_players_to_point(GameState().get_ball_position(), True)[0].player:
+            return True
+        return False
+    def is_second_closest(self, player):
+        if player == closest_players_to_point(GameState().get_ball_position(), True)[1].player:
+            return True
+        return False
+
+    def is_not_closest(self, player):
+        return not(self.is_closest(player))
+
+    def is_not_one_of_the_closests(self, player):
+        # print(player.id)
+        # print(not(self.is_closest(player) or self.is_second_closest(player)))
+        return not(self.is_closest(player) or self.is_second_closest(player))
+
+    def has_kicked(self, player):
+        role = GameState().get_role_by_player_id(player.id)
+        if self.roles_graph[role].get_current_tactic_name() == 'GoKick':
+            return self.roles_graph[role].get_current_tactic().status_flag == Flags.SUCCESS
+        else:
+            return False

ai/STA/Strategy/indiana_jones.py

@@ -19,7 +19,7 @@ def __init__(self, game_state, hard_code=True):
             game_state.map_players_to_roles_by_player_id({
                 Role.FIRST_DEFENCE: 3,
                 Role.MIDDLE: 4,
-                Role.SECOND_DEFENCE: 5,
+                Role.SECOND_DEFENCE: 6,
             })
         indiana = self.game_state.get_player_by_role(Role.MIDDLE)
         indiana_role = Role.MIDDLE
@@ -30,31 +30,32 @@ def __init__(self, game_state, hard_code=True):
 
         # TODO: The position must be update to fit to the current field
         # Positions objectifs d'Indiana Jones
-        goal_left = (Pose(Position(self.game_state.const["FIELD_OUR_GOAL_X_INTERNAL"], 0), 0))
-        goal_right = (Pose(Position(self.game_state.const["FIELD_THEIR_GOAL_X_INTERNAL"], 0), 0))
+        goal_left = (Pose(Position(self.game_state.const["FIELD_OUR_GOAL_X_INTERNAL"], 0), self.game_state.get_player_by_role(indiana_role).pose.orientation))
+        goal_right = (Pose(Position(self.game_state.const["FIELD_THEIR_GOAL_X_INTERNAL"], 0), self.game_state.get_player_by_role(indiana_role).pose.orientation))
 
         # Positions objectifs des obstacles
         y_down = self.game_state.const["FIELD_Y_BOTTOM"] + 500
         y_top = self.game_state.const["FIELD_Y_TOP"] - 500
         x_left = self.game_state.const["FIELD_X_LEFT"] + 500
         x_right = self.game_state.const["FIELD_X_RIGHT"] - 500
 
-        self.add_tactic(indiana_role, GoToPositionPathfinder(self.game_state, indiana, goal_left))
-        self.add_tactic(indiana_role, GoToPositionPathfinder(self.game_state, indiana, goal_right))
+        self.add_tactic(indiana_role, GoToPositionPathfinder(self.game_state, indiana, goal_left, cruise_speed=2))
+        self.add_tactic(indiana_role, GoToPositionPathfinder(self.game_state, indiana, goal_right, cruise_speed=2))
         self.add_condition(indiana_role, 0, 1, partial(self.condition, indiana_role))
         self.add_condition(indiana_role, 1, 0, partial(self.condition, indiana_role))
 
         self.add_tactic(obs_left_role, GoToPositionPathfinder(self.game_state, obs_left,
-                                                            Pose(Position(x_left/2, y_top))))
+                                                            Pose(Position(x_left/2, y_top),
+                                                                 self.game_state.get_player_by_role(obs_left_role).pose.orientation), cruise_speed=2))
         self.add_tactic(obs_left_role, GoToPositionPathfinder(self.game_state, obs_left,
-                                                            Pose(Position(x_left/2, y_down))))
+                                                            Pose(Position(x_left/2, y_down), self.game_state.get_player_by_role(obs_left_role).pose.orientation)))
         self.add_condition(obs_left_role, 0, 1, partial(self.condition, obs_left_role))
         self.add_condition(obs_left_role, 1, 0, partial(self.condition, obs_left_role))
 
         self.add_tactic(obs_right_role, GoToPositionPathfinder(self.game_state,
-                                                             obs_right, Pose(Position(x_right/2, y_top))))
+                                                             obs_right, Pose(Position(x_right/2, y_top), self.game_state.get_player_by_role(obs_right_role).pose.orientation), cruise_speed=2))
         self.add_tactic(obs_right_role, GoToPositionPathfinder(self.game_state,
-                                                             obs_right, Pose(Position(x_right/2, y_down))))
+                                                             obs_right, Pose(Position(x_right/2, y_down), self.game_state.get_player_by_role(obs_right_role).pose.orientation), cruise_speed=2))
         self.add_condition(obs_right_role, 0, 1, partial(self.condition, obs_right_role))
         self.add_condition(obs_right_role, 1, 0, partial(self.condition, obs_right_role))