Merge pull request #27 from project-neon/feat/faster-pass

Feat/faster pass

config.json

@@ -17,7 +17,7 @@
         "event": "Test",
         "team_side": "left",
         "team_color": "blue",
-        "robots_ids": [0, 1, 2, 3],
+        "robots_ids": [0, 1, 2, 3, 4],
         "time_logging": false
     },
     "logger": {

neonfc_ssl/comm/grsim_comm.py

@@ -68,18 +68,19 @@ def update(self):
 
         for robot in cmds:
             robot.global_speed_to_wheel_speed()
+            robot.global_speed_to_local_speed()
             command = commands.robot_commands.add()
             command.wheel1 = robot.wheel_speed[0]
             command.wheel2 = robot.wheel_speed[1]
             command.wheel3 = robot.wheel_speed[2]
             command.wheel4 = robot.wheel_speed[3]
             command.kickspeedx = robot.kick_speed[0]
             command.kickspeedz = robot.kick_speed[1]
-            command.veltangent = 0
-            command.velnormal = 0
-            command.velangular = 0
+            command.veltangent = robot.local_speed[0]
+            command.velnormal = robot.local_speed[1]
+            command.velangular = robot.local_speed[2]
             command.spinner = robot.spinner
-            command.wheelsspeed = True
+            command.wheelsspeed = False
             command.id = robot.robot.robot_id
 
         self.send(commands)

neonfc_ssl/control/control.py

@@ -135,8 +135,14 @@ def run_single_robot(self, command):
 
         # -- Opponent Robots -- #
         [path_planning.add_dynamic_obstacle(r, 0.2, np.array((r.vx, r.vy))) for r in self._match.active_opposites]
-        # [path_planning.add_dynamic_obstacle(r, 0.2, np.array((r.vx, r.vy)))
-        #  for r in self._match.active_robots if r != command.robot]
+
+        if not command.ignore_friendly_robots:
+            [path_planning.add_dynamic_obstacle(r, 0.2, np.array((r.vx, r.vy)))
+             for r in self._match.active_robots if r != command.robot]
+
+        # -- Ball -- #
+        if not command.ignore_ball:
+            path_planning.add_dynamic_obstacle(self._match.ball, 0.2, speed=(0,0))
 
         next_point = path_planning.find_path()
 

neonfc_ssl/entities/robot_command.py

@@ -15,6 +15,8 @@ class RobotCommand:
     kick_speed: Tuple[float, float] = (0, 0)  # vx, vz
     spinner: bool = False
     ignore_area: bool = False
+    ignore_ball: bool = True
+    ignore_friendly_robots: bool = True
 
     # Control
     move_speed: Optional[Tuple[float, float, float]] = None  # vx, vy, omega

neonfc_ssl/game.py

@@ -55,7 +55,7 @@ def __init__(self) -> None:
         self.control = Control(self)
 
         # Output Layer
-        self.comm = GrComm(self)
+        self.comm = SerialComm(self)
 
         # Register exit handler
         atexit.register(self.stop_threads)
@@ -118,6 +118,7 @@ def update(self):
                         self.comm.freeze()
                     else:
                         self.comm.update()
+                    # self.comm.update()
                     t.append(time.time())
                     if self.config['match'].get('time_logging', False):
                         self.logger.info(f"total:  {1/(t[4]-t[0]):.2f} Hz")

neonfc_ssl/referee/ssl_game_controller.py

@@ -4,6 +4,7 @@
 import threading
 import logging
 from google.protobuf.json_format import MessageToJson
+from google.protobuf.message import DecodeError
 from neonfc_ssl.protocols.gc.ssl_gc_referee_message_pb2 import Referee
 
 
@@ -31,8 +32,15 @@ def run(self):
         while self.running:
             c = Referee()
             data = self.referee_sock.recv(1024)
-            c.ParseFromString(data)
-            self._referee_message = json.loads(MessageToJson(c))
+            try:
+                c.ParseFromString(data)
+                self._referee_message = json.loads(MessageToJson(c))
+            except DecodeError as e:
+                self.referee_sock.close()
+                del self.referee_sock
+                self.referee_sock = self._create_socket()
+                self.logger.error(e)
+                self._referee_message = {'command':'HALT'}
         self.stop()
 
     def stop(self):
@@ -53,7 +61,8 @@ def is_stopped(self):
         return self._referee_message.get('command') == 'STOP'
 
     def is_halted(self):
-        return self._referee_message.get('command') == 'HALT'
+        return (self._referee_message.get('command') == 'HALT' or
+                self._referee_message.get('command').startswith('TIMEOUT'))
 
     def simplify(self):
         return {"command": self.get_command(), "team": self.get_team(), "pos": self.get_designated_position()}

neonfc_ssl/skills/passing.py

@@ -21,7 +21,7 @@ def decide(self):
         return RobotCommand(spinner=True, robot=self.robot, move_speed=(0, 0, 0))
 
     def check_complete(self):
-        return self.robot.match.ball.get_speed() < 0.01
+        return self.robot.match.ball.get_speed() < 0.03
 
 
 class StepBack(State):
@@ -185,6 +185,7 @@ def __init__(self, coach, match):
 
         self.wait.add_transition(self.step_back, self.wait.check_complete)
         self.step_back.add_transition(self.turn, self.step_back.check_complete)
+        self.step_back.add_transition(self.step_forward, self.turn.check_complete)
         self.turn.add_transition(self.step_forward, self.turn.check_complete)
         self.step_forward.add_transition(self.passing, self.step_forward.check_complete)
 
@@ -206,11 +207,11 @@ def decide(self):
 
     @staticmethod
     def start_pass(robot, ball):
-        return distance_between_points(robot, ball) < 0.1
+        return distance_between_points(robot, ball) < 0.12
 
     @staticmethod
     def stop_pass(robot, ball):
-        return distance_between_points(robot, ball) > 0.15
+        return distance_between_points(robot, ball) > 0.18
 
 
 class ChipPass(State):

neonfc_ssl/state_controller/state_controller.py

@@ -1,7 +1,9 @@
 from neonfc_ssl.algorithms.fsm import State
 from neonfc_ssl.commons.math import distance_between_points
 from time import time
+from dataclasses import dataclass
 import logging
+from typing import Optional
 
 
 class GameState(State):
@@ -12,12 +14,20 @@ def __init__(self, name):
         self.ball_initial_position = None
         self.color = None
         self.position = None
+        self.last_state: Optional[LastStateInfo] = None
 
-    def start(self, match, color, position):
+    def start(self, match, color, position, last_state):
         self.start_time = time()
         self.ball_initial_position = (match.ball.x, match.ball.y)
         self.color = color
         self.position = position
+        self.last_state = last_state
+
+
+@dataclass
+class LastStateInfo:
+    first_touch: bool
+    first_touch_id: int
 
 
 class StateController:
@@ -31,6 +41,8 @@ def __init__(self, match):
         console_handler.setFormatter(log_formatter)
         self.logger.addHandler(console_handler)
 
+        self.last_state: Optional[LastStateInfo] = None
+
         # Following appendix B found in: https://robocup-ssl.github.io/ssl-rules/sslrules.pdf
         self.states = {
             'Halt': GameState('Halt'),
@@ -51,14 +63,16 @@ def trig(cmd, **kwargs):
             return trig
 
         def after_secs(delay):
+            @self.save_info(False)
             def trig(origin, **kwargs):
                 return time() - origin.start_time >= delay
             return trig
 
         after_10 = after_secs(10)
 
-        def ball_moved(origin, ball, **kwargs):
-            return distance_between_points(ball, origin.ball_initial_position) <= 0.05
+        @self.save_info(True)
+        def ball_moved(ball, **kwargs):
+            return ball.get_speed() >= 0.07
 
         # _ -> Halt
         halt = on_ref_message('HALT')
@@ -122,14 +136,33 @@ def ball_moved(origin, ball, **kwargs):
         self.states['FreeKick'].add_transition(self.states['Run'], after_10)
 
         self.current_state = self.states['Halt']
-        self.current_state.start(self._match, None, None)
+        self.current_state.start(self._match, None, None, None)
+
+    def save_info(self, touched):
+        def wrapper(f):
+            def wrapped(poss, *args, **kwargs):
+                out = f(*args, **kwargs)
+                if out:
+                    self.last_state = LastStateInfo(touched, poss.current_closest.robot_id if touched else None)
+                return out
+            return wrapped
+        return wrapper
 
     def update(self, ref):
-        next_state = self.current_state.update(origin=self.current_state, cmd=ref['command'], ball=self._match.ball)
+        next_state = self.current_state.update(
+            origin=self.current_state,
+            cmd=ref['command'],
+            ball=self._match.ball,
+            poss=self._match.possession
+        )
+
+        # TODO: This lacks some way to reset the last touch info when the robot that did the initial touch can touch again
+
         if next_state != self.current_state:
             self.logger.info(f"Changing state {self.current_state.name} -> {next_state.name}")
             self.current_state = next_state
-            self.current_state.start(self._match, ref['team'], ref['pos'])
+            self.current_state.start(self._match, ref['team'], ref['pos'], self.last_state)
+            self.last_state = None
 
     def is_stopped(self):
         return self.current_state.name in ["Stop", "PrepareKickOff", "BallPlacement", "PreparePenalty"]

neonfc_ssl/strategies/__init__.py

@@ -13,3 +13,4 @@
 from neonfc_ssl.strategies.prepare_penalty import PrepPenalty
 from neonfc_ssl.strategies.prepare_gk_penalty import PrepGKPenalty
 from neonfc_ssl.strategies.prepare_bh_penalty import PrepBHPenalty
+from neonfc_ssl.strategies.prepare_secondary_kickoff import PrepSecondKickoff

neonfc_ssl/strategies/ball_holder.py

@@ -34,7 +34,8 @@ def __init__(self, coach, match):
             'go_to_ball': GoToBall(coach, match),
             'wait': Wait(coach, match),
             'shoot': Shoot(coach, match),
-            'dribble': Dribble(coach, match)
+            'dribble': Dribble(coach, match),
+            'wait_outside_area': MoveToPose(coach, match)
         }
 
         def close_to_ball():
@@ -46,19 +47,31 @@ def wrapped():
 
             return wrapped
 
+        def and_func(f1, f2):
+            def wrapped():
+                return f1() and f2()
+
+            return wrapped
+
         def wrapped_stop_pass():
             return SimplePass.stop_pass(robot=self._robot, ball=self._match.ball)
 
-        self.states['pass'].add_transition(self.states['go_to_ball'], wrapped_stop_pass)
-        self.states['wait'].add_transition(self.states['go_to_ball'], not_func(close_to_ball))
-        self.states['shoot'].add_transition(self.states['go_to_ball'], not_func(close_to_ball))
+        self.states['pass'].add_transition(self.states['go_to_ball'], and_func(wrapped_stop_pass, not_func(self.ball_inside_area)))
+        self.states['wait'].add_transition(self.states['go_to_ball'], and_func(not_func(close_to_ball), not_func(self.ball_inside_area)))
+        self.states['shoot'].add_transition(self.states['go_to_ball'], and_func(not_func(close_to_ball), not_func(self.ball_inside_area)))
         self.states['dribble'].add_transition(self.states['wait'], not_func(close_to_ball))
         self.states['go_to_ball'].add_transition(self.states['wait'], close_to_ball)
         self.states['dribble'].add_transition(self.states['pass'], self.pass_transition)
         self.states['dribble'].add_transition(self.states['shoot'], self.shoot_transition)
-        self.states['wait'].add_transition(self.states['pass'], self.pass_transition)
-        self.states['wait'].add_transition(self.states['shoot'], self.shoot_transition)
-        self.states['wait'].add_transition(self.states['dribble'], self.dribble_transition)
+        self.states['wait'].add_transition(self.states['pass'], and_func(self.pass_transition, not_func(self.ball_inside_area)))
+        self.states['wait'].add_transition(self.states['shoot'], and_func(self.shoot_transition, not_func(self.ball_inside_area)))
+        self.states['wait'].add_transition(self.states['dribble'], and_func(self.dribble_transition, not_func(self.ball_inside_area)))
+        self.states['go_to_ball'].add_transition(self.states['wait_outside_area'], self.ball_inside_area)
+        self.states['pass'].add_transition(self.states['wait_outside_area'], self.ball_inside_area)
+        self.states['shoot'].add_transition(self.states['wait_outside_area'], self.ball_inside_area)
+        self.states['dribble'].add_transition(self.states['wait_outside_area'], self.ball_inside_area)
+        self.states['wait'].add_transition(self.states['wait_outside_area'], self.ball_inside_area)
+        self.states['wait_outside_area'].add_transition(self.states['wait'], not_func(self.ball_inside_area))
 
         self.message = None
         self.passing_to = None
@@ -85,6 +98,8 @@ def decide(self):
                 self.active.start(self._robot, target=self._pass_target)
             elif self.active.name == "Dribble":
                 self.active.start(self._robot, target=self._pass_target)
+            elif self.active.name == "MoveToPose":
+                self.active.start(self._robot, target=[self._match.field.fieldLength/2, self._match.field.fieldWidth/2, 0])
             else:
                 self.active.start(self._robot)
 
@@ -97,7 +112,9 @@ def decide(self):
             ]).encode('ascii')
             self.sock.sendto(MESSAGE, (self.UDP_IP, self.UDP_PORT))
 
-        return self.active.decide()
+        com = self.active.decide()
+        com.ignore_friendly_robots = False
+        return com
 
     def update_shooting_value(self):
         if self._robot.x > 9:
@@ -153,14 +170,37 @@ def pass_transition(self):
 
         return False
 
+    def ball_inside_area(self):
+        f = self._match.field
+        b = self._match.ball
+        return (b.x <= f.penaltyAreaDepth and
+                (f.penaltyAreaWidth - f.fieldWidth)/2 <= b.y <= (f.penaltyAreaWidth + f.fieldWidth)/2)
+
     def shoot_transition(self):
         if self._shooting_value >= self._pass_value:
             return True
 
         return False
 
     def _passing_targets(self):
+        return self._close_targets()
+
+    def _close_targets(self):
         dx, dy = 0.1, 0.1
+
+        r = .7
+        lwx, upx = min(self._robot.x + r, self._match.field.fieldLength), max(self._robot.x - r, 0)
+        lwy, upy = min(self._robot.y + r, self._match.field.fieldWidth), max(self._robot.y - r, 0)
+
+        x = np.linspace(lwx + dx, upx - dx, 10)
+        y = np.linspace(lwy + dy, upy - dy, 10)
+        xs, ys = np.meshgrid(x, y)
+
+        return np.transpose(np.array([xs.flatten(), ys.flatten()]))
+
+    def _attack_targets(self):
+        dx, dy = 0.1, 0.1
+
         # Area 0
         lwx, upx = 4.5, 9
         lwy, upy = 0, 2