From 3e81acdec065de707b1d16c7acc442c053b8f7db Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Wed, 16 Jun 2021 16:23:44 -0400
Subject: [PATCH 01/28] basic defense added

---
 rj_gameplay/rj_gameplay/gameplay_node.py      | 19 +++---
 rj_gameplay/rj_gameplay/play/basic_defense.py | 61 +++++++++++++++++++
 2 files changed, 71 insertions(+), 9 deletions(-)
 create mode 100644 rj_gameplay/rj_gameplay/play/basic_defense.py

diff --git a/rj_gameplay/rj_gameplay/gameplay_node.py b/rj_gameplay/rj_gameplay/gameplay_node.py
index 6526507d441..a3387077a4b 100644
--- a/rj_gameplay/rj_gameplay/gameplay_node.py
+++ b/rj_gameplay/rj_gameplay/gameplay_node.py
@@ -12,7 +12,7 @@
 from stp.global_parameters import GlobalParameterClient
 import numpy as np
 from rj_gameplay.action.move import Move
-from rj_gameplay.play import defensive_clear 
+from rj_gameplay.play import basic_defense
 from typing import List, Optional, Tuple
 
 NUM_ROBOTS = 16
@@ -25,7 +25,8 @@ def select(self, world_state: rc.WorldState) -> None:
 
 class TestPlaySelector(situation.IPlaySelector):
     def select(self, world_state: rc.WorldState) -> Tuple[situation.ISituation, stp.play.IPlay]:
-        return (None, line_up.LineUp())
+        return (None, basic_defense.BasicDefense())
+
 
 class GameplayNode(Node):
     """
@@ -48,11 +49,11 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
 
         for i in range(NUM_ROBOTS):
             self.robot_state_subs[i] = self.create_subscription(msg.RobotStatus, '/radio/robot_status/robot_'+str(i), self.create_partial_robots, 10)
- 
+
         for i in range(NUM_ROBOTS):
             self.robot_intent_pubs[i] = self.create_publisher(msg.RobotIntent, '/gameplay/robot_intent/robot_'+str(i), 10)
 
-        
+
         self.get_logger().info("Gameplay node started")
         self.world_state = world_state
         self.partial_world_state: conv.PartialWorldState = None
@@ -87,7 +88,7 @@ def create_partial_robots(self, msg: msg.RobotStatus) -> None:
             robot = conv.robotstatus_to_partial_robot(msg)
             index = robot.robot_id
             self.robot_statuses[index] = robot
-        
+
     def create_game_info(self, msg: msg.GameState) -> None:
         """
         Create game info object from Game State message
@@ -110,7 +111,7 @@ def get_world_state(self) -> rc.WorldState:
         if self.partial_world_state is not None and self.field is not None and len(self.robot_statuses) == len(self.partial_world_state.our_robots):
 
             self.world_state = conv.worldstate_creator(self.partial_world_state, self.robot_statuses, self.game_info, self.field)
-        
+
         return self.world_state
 
     def gameplay_tick(self) -> None:
@@ -132,13 +133,13 @@ def gameplay_tick(self) -> None:
             our_penalty = geo_msg.Rect()
             top_left = geo_msg.Point(x=self.field.penalty_long_dist_m/2 + self.field.line_width_m, y=0.0)
             bot_right = geo_msg.Point(x=-self.field.penalty_long_dist_m/2 - self.field.line_width_m, y=self.field.penalty_short_dist_m)
-            our_penalty.pt = [top_left, bot_right] 
+            our_penalty.pt = [top_left, bot_right]
 
             # create their_penalty rect
             their_penalty = geo_msg.Rect()
             bot_left = geo_msg.Point(x=self.field.penalty_long_dist_m/2 + self.field.line_width_m, y=self.field.length_m)
             top_right = geo_msg.Point(x=-self.field.penalty_long_dist_m/2 - self.field.line_width_m, y=self.field.length_m - self.field.penalty_short_dist_m)
-            their_penalty.pt = [bot_left, top_right] 
+            their_penalty.pt = [bot_left, top_right]
 
             # publish Rect shape to global_obstacles topic
             global_obstacles = geo_msg.ShapeSet()
@@ -146,7 +147,7 @@ def gameplay_tick(self) -> None:
             self.global_obstacles_pub.publish(global_obstacles)
         else:
             self.get_logger().warn("World state was none!")
-    
+
     def tick_override_actions(self, world_state) -> None:
         for i in range(0,NUM_ROBOTS):
             if self.override_actions[i] is not None:
diff --git a/rj_gameplay/rj_gameplay/play/basic_defense.py b/rj_gameplay/rj_gameplay/play/basic_defense.py
new file mode 100644
index 00000000000..7b38928cf72
--- /dev/null
+++ b/rj_gameplay/rj_gameplay/play/basic_defense.py
@@ -0,0 +1,61 @@
+import stp.play as play
+import stp.tactic as tactic
+
+from rj_gameplay.tactic import wall_tactic, nmark_tactic
+import stp.skill as skill
+import stp.role as role
+from stp.role.assignment.naive import NaiveRoleAssignment
+import stp.rc as rc
+from typing import Dict, Generic, Iterator, List, Optional, Tuple, Type, TypeVar
+
+class BasicDefense(play.IPlay):
+    """For when we don't have the ball and are trying to stop the opponent from scoring.
+    """
+
+    def __init__(self):
+        self.tactics = [
+            wall_tactic.WallTactic(3),
+            nmark_tactic.NMarkTactic(2)
+            # TODO: add goalie tactic here
+        ]
+
+        self.role_assigner = NaiveRoleAssignment()
+
+    def compute_props(self, prev_props):
+        pass
+
+    def tick(
+        self,
+        world_state: rc.WorldState,
+        prev_results: role.assignment.FlatRoleResults,
+        props,
+    ) -> Tuple[Dict[Type[tactic.SkillEntry], List[role.RoleRequest]], List[tactic.SkillEntry]]:
+
+        # Get role requests from all tactics and put them into a dictionary
+        role_requests: play.RoleRequests = {
+            tactic: tactic.get_requests(world_state, None)
+            for tactic in self.tactics
+        }
+
+        # Flatten requests and use role assigner on them
+        flat_requests = play.flatten_requests(role_requests)
+        flat_results = self.role_assigner.assign_roles(flat_requests, world_state, prev_results)
+        role_results = play.unflatten_results(flat_results)
+
+        # Get list of all SkillEntries from all tactics
+        skills = []
+        for tactic in self.tactics:
+            skills += tactic.tick(role_results[tactic])
+
+        # Get all role assignments
+        # SkillEntry to (list of?) RoleResult
+        skill_dict = {}
+        for tactic in self.tactics:
+            skill_dict.update(role_results[tactic])
+
+        return (skill_dict, skills)
+
+    def is_done(self, world_state):
+        # returns done when all tactics are done
+        # TODO: change all is_done() to use all()?
+        return all([tactic.is_done(world_state) for tactic in self.tactics])

From 422be41a8e938309afcfdc2ef7461e6170386571 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Wed, 16 Jun 2021 16:24:05 -0400
Subject: [PATCH 02/28] ran make pretty lines

---
 rj_gameplay/rj_gameplay/gameplay_node.py      |  1 -
 rj_gameplay/rj_gameplay/play/basic_defense.py |  9 ++-
 rj_gameplay/rj_gameplay/play/wall_ball.py     | 15 +++--
 rj_gameplay/rj_gameplay/skill/move.py         |  4 +-
 rj_gameplay/rj_gameplay/tactic/wall_tactic.py | 58 +++++++++----------
 5 files changed, 44 insertions(+), 43 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/gameplay_node.py b/rj_gameplay/rj_gameplay/gameplay_node.py
index a3387077a4b..e324a1f89ea 100644
--- a/rj_gameplay/rj_gameplay/gameplay_node.py
+++ b/rj_gameplay/rj_gameplay/gameplay_node.py
@@ -53,7 +53,6 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
         for i in range(NUM_ROBOTS):
             self.robot_intent_pubs[i] = self.create_publisher(msg.RobotIntent, '/gameplay/robot_intent/robot_'+str(i), 10)
 
-
         self.get_logger().info("Gameplay node started")
         self.world_state = world_state
         self.partial_world_state: conv.PartialWorldState = None
diff --git a/rj_gameplay/rj_gameplay/play/basic_defense.py b/rj_gameplay/rj_gameplay/play/basic_defense.py
index 7b38928cf72..eb1c79fb1b1 100644
--- a/rj_gameplay/rj_gameplay/play/basic_defense.py
+++ b/rj_gameplay/rj_gameplay/play/basic_defense.py
@@ -8,10 +8,10 @@
 import stp.rc as rc
 from typing import Dict, Generic, Iterator, List, Optional, Tuple, Type, TypeVar
 
+
 class BasicDefense(play.IPlay):
     """For when we don't have the ball and are trying to stop the opponent from scoring.
     """
-
     def __init__(self):
         self.tactics = [
             wall_tactic.WallTactic(3),
@@ -29,7 +29,8 @@ def tick(
         world_state: rc.WorldState,
         prev_results: role.assignment.FlatRoleResults,
         props,
-    ) -> Tuple[Dict[Type[tactic.SkillEntry], List[role.RoleRequest]], List[tactic.SkillEntry]]:
+    ) -> Tuple[Dict[Type[tactic.SkillEntry], List[role.RoleRequest]],
+               List[tactic.SkillEntry]]:
 
         # Get role requests from all tactics and put them into a dictionary
         role_requests: play.RoleRequests = {
@@ -39,7 +40,9 @@ def tick(
 
         # Flatten requests and use role assigner on them
         flat_requests = play.flatten_requests(role_requests)
-        flat_results = self.role_assigner.assign_roles(flat_requests, world_state, prev_results)
+        flat_results = self.role_assigner.assign_roles(flat_requests,
+                                                       world_state,
+                                                       prev_results)
         role_results = play.unflatten_results(flat_results)
 
         # Get list of all SkillEntries from all tactics
diff --git a/rj_gameplay/rj_gameplay/play/wall_ball.py b/rj_gameplay/rj_gameplay/play/wall_ball.py
index 0c008e6a6b0..12e2eef4ad3 100644
--- a/rj_gameplay/rj_gameplay/play/wall_ball.py
+++ b/rj_gameplay/rj_gameplay/play/wall_ball.py
@@ -9,11 +9,11 @@
 from typing import Dict, Generic, Iterator, List, Optional, Tuple, Type, TypeVar
 import numpy as np
 
+
 class WallBall(play.IPlay):
     """
     Test play for the wall tactic. Directs robots to form a wall between the ball and goal.
     """
-
     def __init__(self):
         # defaults to walling between ball pos and goal pos
         self.wall_tactic = wall_tactic.WallTactic(3)
@@ -27,14 +27,18 @@ def tick(
         world_state: rc.WorldState,
         prev_results: role.assignment.FlatRoleResults,
         props,
-    ) -> Tuple[Dict[Type[tactic.SkillEntry], List[role.RoleRequest]], List[tactic.SkillEntry]]:
+    ) -> Tuple[Dict[Type[tactic.SkillEntry], List[role.RoleRequest]],
+               List[tactic.SkillEntry]]:
         # Get role requests from all tactics and put them into a dictionary
         role_requests: play.RoleRequests = {}
-        role_requests[self.wall_tactic] = self.wall_tactic.get_requests(world_state, None)
+        role_requests[self.wall_tactic] = self.wall_tactic.get_requests(
+            world_state, None)
 
         # Flatten requests and use role assigner on them
         flat_requests = play.flatten_requests(role_requests)
-        flat_results = self.role_assigner.assign_roles(flat_requests, world_state, prev_results)
+        flat_results = self.role_assigner.assign_roles(flat_requests,
+                                                       world_state,
+                                                       prev_results)
         role_results = play.unflatten_results(flat_results)
 
         # Get list of all skills with assigned roles from tactics
@@ -44,6 +48,5 @@ def tick(
 
         return (skill_dict, skills)
 
-    def is_done(self ,world_state):
+    def is_done(self, world_state):
         return self.wall_tactic.is_done(world_state)
-
diff --git a/rj_gameplay/rj_gameplay/skill/move.py b/rj_gameplay/rj_gameplay/skill/move.py
index d73bde00f46..8bcb644b852 100644
--- a/rj_gameplay/rj_gameplay/skill/move.py
+++ b/rj_gameplay/rj_gameplay/skill/move.py
@@ -24,7 +24,6 @@ class IMove(skill.ISkill, ABC):
 A skill version of move so that actions don't have to be called in tactics
 """
 class Move(IMove):
-    
     def __init__(self,
             robot : rc.Robot = None,
             target_point : np.ndarray = np.array([0.0,0.0]),
@@ -42,7 +41,8 @@ def __init__(self,
         self.root.setup_with_descendants()
         self.__name__ = 'move skill'
 
-    def tick(self, robot: rc.Robot, world_state: rc.WorldState): #returns dict of robot and actions
+    def tick(self, robot: rc.Robot,
+             world_state: rc.WorldState):  #returns dict of robot and actions
         self.robot = robot
         self.move.target_point = self.target_point
         # TODO: figure out why AttributeError: 'Move object has no attribute face_point' keeps popping up (move action does have a face_point)
diff --git a/rj_gameplay/rj_gameplay/tactic/wall_tactic.py b/rj_gameplay/rj_gameplay/tactic/wall_tactic.py
index 47ac2637fbd..34642172f02 100644
--- a/rj_gameplay/rj_gameplay/tactic/wall_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/wall_tactic.py
@@ -18,29 +18,29 @@
 from stp.utils.constants import RobotConstants, BallConstants
 import stp.global_parameters as global_parameters
 
+
 class wall_cost(role.CostFn):
     """Cost function for role request.
     """
-    def __init__(self, wall_pt: np.ndarray=None):
+    def __init__(self, wall_pt: np.ndarray = None):
         self.wall_pt = wall_pt
 
-    def __call__(
-        self,
-        robot: rc.Robot,
-        prev_result: Optional["RoleResult"],
-        world_state: rc.WorldState
-    ) -> float:
+    def __call__(self, robot: rc.Robot, prev_result: Optional["RoleResult"],
+                 world_state: rc.WorldState) -> float:
 
         if robot is None or self.wall_pt is None:
             return 0
 
         if not robot.visible:
-            return 99999 # float('inf') threw ValueError
+            return 99999  # float('inf') threw ValueError
 
         # costs should be in seconds, not dist
-        return np.linalg.norm(robot.pose[0:2]-self.wall_pt) / global_parameters.soccer.robot.max_speed
+        return np.linalg.norm(robot.pose[0:2] - self.wall_pt
+                              ) / global_parameters.soccer.robot.max_speed
+
 
-def find_wall_pts(num_wallers: int, world_state: rc.WorldState) -> List[np.ndarray]:
+def find_wall_pts(num_wallers: int,
+                  world_state: rc.WorldState) -> List[np.ndarray]:
     """Calculates num_wallers points to form a wall between the ball and goal.
     :return list of wall_pts (as numpy arrays)
     """
@@ -55,7 +55,7 @@ def find_wall_pts(num_wallers: int, world_state: rc.WorldState) -> List[np.ndarr
     box_w = world_state.field.penalty_long_dist_m
     box_h = world_state.field.penalty_short_dist_m
     line_w = world_state.field.line_width_m
-    DIST_FROM_DEF = RobotConstants.RADIUS + line_w + np.hypot(box_w/2, box_h)
+    DIST_FROM_DEF = RobotConstants.RADIUS + line_w + np.hypot(box_w / 2, box_h)
 
     # get direction vec
     dir_vec = (ball_pt - goal_pt) / np.linalg.norm(ball_pt - goal_pt)
@@ -67,32 +67,28 @@ def find_wall_pts(num_wallers: int, world_state: rc.WorldState) -> List[np.ndarr
 
     # set wall points in middle out pattern, given wall dir vector and WALL_SPACING constant
     wall_pts = [mid_pt]
-    for i in range(num_wallers-1):
-        mult = i//2 + 1
-        delta = (mult * (2 * RobotConstants.RADIUS + WALL_SPACING)) * wall_vec 
+    for i in range(num_wallers - 1):
+        mult = i // 2 + 1
+        delta = (mult * (2 * RobotConstants.RADIUS + WALL_SPACING)) * wall_vec
         if i % 2: delta = -delta
         wall_pts.append(mid_pt + delta)
 
     return wall_pts
 
-class WallTactic(tactic.ITactic):
 
+class WallTactic(tactic.ITactic):
     def __init__(self, num_wallers: int):
 
         self.num_wallers = num_wallers
 
         # create move SkillEntry for every robot
         self.move_list = [
-            tactic.SkillEntry(move.Move()) 
-            for _ in range(num_wallers)
+            tactic.SkillEntry(move.Move()) for _ in range(num_wallers)
         ]
 
         # create empty cost_list (filled in get_requests)
-        self.cost_list = [
-            wall_cost()
-            for _ in range(self.num_wallers)
-        ]
-        
+        self.cost_list = [wall_cost() for _ in range(self.num_wallers)]
+
     def compute_props(self):
         pass
 
@@ -102,9 +98,8 @@ def create_request(self, **kwargs) -> role.RoleRequest:
         """
         pass
 
-    def get_requests(
-        self, world_state: rc.WorldState, props
-    ) -> List[tactic.RoleRequests]:
+    def get_requests(self, world_state: rc.WorldState,
+                     props) -> List[tactic.RoleRequests]:
         """
         :return: A list of role requests for move skills needed
         """
@@ -120,7 +115,8 @@ def get_requests(
 
         # create RoleRequest for each SkillEntry
         role_requests = {
-            self.move_list[i]: [role.RoleRequest(role.Priority.HIGH, False, self.cost_list[i])]
+            self.move_list[i]:
+            [role.RoleRequest(role.Priority.HIGH, False, self.cost_list[i])]
             for i in range(self.num_wallers)
         }
 
@@ -130,18 +126,18 @@ def get_requests(
 
         return role_requests
 
-    def tick(self, role_results: tactic.RoleResults) -> List[tactic.SkillEntry]:
+    def tick(self,
+             role_results: tactic.RoleResults) -> List[tactic.SkillEntry]:
         """
         :return: A list of skills depending on which roles are filled
         """
 
-        # create list of skills based on if RoleResult exists for SkillEntry 
+        # create list of skills based on if RoleResult exists for SkillEntry
         skills = [
-            move_skill_entry
-            for move_skill_entry in self.move_list
+            move_skill_entry for move_skill_entry in self.move_list
             if role_results[move_skill_entry][0]
         ]
-        
+
         return skills
 
     def is_done(self, world_state):

From 97844cb12f1f7186d3bd71e3a0e912146224d977 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Wed, 16 Jun 2021 21:39:32 -0400
Subject: [PATCH 03/28] added goalie_id field to world_state.game_info

---
 rj_gameplay/rj_gameplay/gameplay_node.py       |  8 +++++++-
 rj_gameplay/stp/rc.py                          | 18 ++++++++++++++++--
 rj_gameplay/stp/utils/world_state_converter.py |  8 ++------
 3 files changed, 25 insertions(+), 9 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/gameplay_node.py b/rj_gameplay/rj_gameplay/gameplay_node.py
index e324a1f89ea..744d30923d2 100644
--- a/rj_gameplay/rj_gameplay/gameplay_node.py
+++ b/rj_gameplay/rj_gameplay/gameplay_node.py
@@ -39,7 +39,7 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
         self.world_state_sub = self.create_subscription(msg.WorldState, '/vision_filter/world_state', self.create_partial_world_state, 10)
         self.field_dimensions = self.create_subscription(msg.FieldDimensions, '/config/field_dimensions', self.create_field, 10)
         self.game_info = self.create_subscription(msg.GameState, '/referee/game_state', self.create_game_info, 10)
-
+        self.goalie_id_sub = self.create_subscription(msg.Goalie, '/referee/our_goalie', self.set_goalie_id, 10)
 
         self.robot_state_subs = [None] * NUM_ROBOTS
         self.robot_intent_pubs = [None] * NUM_ROBOTS
@@ -102,6 +102,12 @@ def create_field(self, msg: msg.FieldDimensions) -> None:
         if msg is not None:
             self.field = conv.field_msg_to_field(msg)
 
+    def set_goalie_id(self, msg: msg.Goalie) -> None:
+        """
+        Set goalie id based on goalie msg
+        """
+        if msg is not None and self.game_info is not None:
+            self.game_info.set_goalie_id(msg.goalie_id)
 
     def get_world_state(self) -> rc.WorldState:
         """
diff --git a/rj_gameplay/stp/rc.py b/rj_gameplay/stp/rc.py
index 2650ca3d9e0..740a8227cfd 100644
--- a/rj_gameplay/stp/rc.py
+++ b/rj_gameplay/stp/rc.py
@@ -425,19 +425,21 @@ def goal_height_m(self) -> float:
 class GameInfo:
     """State of the soccer game"""
 
-    __slots__ = ["__period", "__state", "__restart", "__our_restart"]
+    __slots__ = ["__period", "__state", "__restart", "__our_restart", "__goalie_id"]
 
     __period: GamePeriod
     __state: GameState
     __restart: GameRestart
     __our_restart: bool
+    __goalie_id: int
 
     def __init__(self, period: GamePeriod, state: GameState,
-                 restart: GameRestart, our_restart: bool):
+            restart: GameRestart, our_restart: bool, goalie_id: int):
         self.__period = period
         self.__state = state
         self.__restart = restart
         self.__our_restart = our_restart
+        self.__goalie_id = goalie_id
 
     @property
     def period(self) -> GamePeriod:
@@ -473,6 +475,18 @@ def our_restart(self) -> bool:
 
         return self.__our_restart
 
+    @property
+    def goalie_id(self) -> int:
+        """
+        :return: id (int) of our designated goalie
+        """
+        return self.__goalie_id
+
+    def set_goalie_id(self, goalie_id: int) -> None:
+        """Sets goalie id. (Created to maintain private data convention.)
+        """
+        self.__goalie_id = goalie_id
+
     def is_restart(self) -> bool:
         """
         :return: True if there is a restart.
diff --git a/rj_gameplay/stp/utils/world_state_converter.py b/rj_gameplay/stp/utils/world_state_converter.py
index ed74cfac594..8d8cefedae4 100644
--- a/rj_gameplay/stp/utils/world_state_converter.py
+++ b/rj_gameplay/stp/utils/world_state_converter.py
@@ -130,8 +130,7 @@ def ballstate_to_ball(ball_msg: msg.BallState) -> rc.Ball:
     vel = np.array([dx,dy])
 
     visible = ball_msg.visible
-
-    ball = rc.Ball(pos,vel, visible)
+    ball = rc.Ball(pos, vel, visible)
 
     return ball
 
@@ -141,14 +140,11 @@ def gamestate_to_gameinfo(game_state_msg: msg.GameState) -> rc.GameInfo:
     """
 
     period = game_state_msg.period
-
     state = game_state_msg.state
-
     restart = game_state_msg.restart
-
     our_restart = game_state_msg.our_restart
 
-    game_info = rc.GameInfo(period, state, restart, our_restart)
+    game_info = rc.GameInfo(period, state, restart, our_restart, None) # goalie_id set later
 
     return game_info
 

From f1f4701945cecf6f58e74349aa5ece5b5d5be9d5 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Wed, 16 Jun 2021 21:39:56 -0400
Subject: [PATCH 04/28] rm debug prints from wall

---
 rj_gameplay/rj_gameplay/tactic/wall_tactic.py | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/tactic/wall_tactic.py b/rj_gameplay/rj_gameplay/tactic/wall_tactic.py
index 34642172f02..1de7929332a 100644
--- a/rj_gameplay/rj_gameplay/tactic/wall_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/wall_tactic.py
@@ -120,10 +120,6 @@ def get_requests(self, world_state: rc.WorldState,
             for i in range(self.num_wallers)
         }
 
-        for se, rr in role_requests.items():
-            print(se.skill.robot)
-            # print(rr)
-
         return role_requests
 
     def tick(self,

From 7bd18f52b823ef22f5867b97e6284f0f758d372a Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Wed, 16 Jun 2021 21:40:16 -0400
Subject: [PATCH 05/28] rm debug print kick

---
 rj_gameplay/rj_gameplay/action/kick.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rj_gameplay/rj_gameplay/action/kick.py b/rj_gameplay/rj_gameplay/action/kick.py
index 4c4c2cab405..166d492e2b6 100644
--- a/rj_gameplay/rj_gameplay/action/kick.py
+++ b/rj_gameplay/rj_gameplay/action/kick.py
@@ -24,7 +24,7 @@ def __init__(self, point: np.ndarray):
         #for stub
 
     def tick(self, robot: rc.Robot, ctx: action.Ctx) -> None:
-        print('robot:', robot.id, 'kicking')
+        # print('robot:', robot.id, 'kicking')
         self.count += 1
 
     def done(self) -> bool:

From 4d3966db481a4d857a0506e08dc9ec893ec8445b Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Wed, 16 Jun 2021 21:42:50 -0400
Subject: [PATCH 06/28] init goalie tactic

---
 rj_gameplay/rj_gameplay/play/basic_defense.py |   6 +-
 .../rj_gameplay/tactic/goalie_tactic.py       | 105 ++++++++++++++++++
 2 files changed, 108 insertions(+), 3 deletions(-)
 create mode 100644 rj_gameplay/rj_gameplay/tactic/goalie_tactic.py

diff --git a/rj_gameplay/rj_gameplay/play/basic_defense.py b/rj_gameplay/rj_gameplay/play/basic_defense.py
index eb1c79fb1b1..8715a0bc39c 100644
--- a/rj_gameplay/rj_gameplay/play/basic_defense.py
+++ b/rj_gameplay/rj_gameplay/play/basic_defense.py
@@ -1,7 +1,7 @@
 import stp.play as play
 import stp.tactic as tactic
 
-from rj_gameplay.tactic import wall_tactic, nmark_tactic
+from rj_gameplay.tactic import wall_tactic, nmark_tactic, goalie_tactic
 import stp.skill as skill
 import stp.role as role
 from stp.role.assignment.naive import NaiveRoleAssignment
@@ -15,8 +15,8 @@ class BasicDefense(play.IPlay):
     def __init__(self):
         self.tactics = [
             wall_tactic.WallTactic(3),
-            nmark_tactic.NMarkTactic(2)
-            # TODO: add goalie tactic here
+            nmark_tactic.NMarkTactic(2),
+            goalie_tactic.GoalieTactic()
         ]
 
         self.role_assigner = NaiveRoleAssignment()
diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
new file mode 100644
index 00000000000..1e9b115db16
--- /dev/null
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -0,0 +1,105 @@
+"""Tactic to build a wall between mark pt (e.g. ball) and defense pt (e.g. goal)."""
+
+from dataclasses import dataclass
+from typing import List, Optional
+from typing import Dict, Generic, List, Optional, Tuple, Type, TypeVar
+
+import stp.action as action
+import stp.rc as rc
+import stp.tactic as tactic
+import stp.role as role
+
+import rj_gameplay.eval
+import rj_gameplay.skill as skills
+from rj_gameplay.skill import move
+import stp.skill as skill
+import numpy as np
+# TODO: replace w/ global param server
+from stp.utils.constants import RobotConstants, BallConstants
+import stp.global_parameters as global_parameters
+
+
+class goalie_cost(role.CostFn):
+    """Cost function for role request. Want only the goalie to be selected.
+    """
+    def __call__(self, robot: rc.Robot, prev_result: Optional["RoleResult"],
+                 world_state: rc.WorldState) -> float:
+
+        if world_state.game_info is not None:
+            if robot.id == world_state.game_info.goalie_id:
+                return -1.0
+        return 999.0
+
+def get_goalie_pt(world_state: rc.WorldState) -> np.ndarray:
+    """Finds point for goalie to best intercept a shot.
+    :return numpy point
+    """
+    # TODO: param server this const
+    # TODO: param server any constant from stp/utils/constants.py (this includes BallConstants)
+    ball_pt = world_state.ball.pos
+    goal_pt = world_state.field.our_goal_loc
+
+    PCT_TO_BALL = 0.50
+
+    # get direction vec to ball
+    dir_vec = (ball_pt - goal_pt) / np.linalg.norm(ball_pt - goal_pt)
+    wall_vec = np.array([dir_vec[1], -dir_vec[0]])
+
+    # find and return pt
+    mid_pt = goal_pt + (dir_vec * PCT_TO_BALL * np.linalg.norm(ball_pt - goal_pt))
+    return mid_pt
+
+
+class GoalieTactic(tactic.ITactic):
+    def __init__(self):
+
+        # create move SkillEntry
+        self.move_se = tactic.SkillEntry(move.Move())
+        # TODO: intercept skill
+
+        # TODO: rename cost_list to role_cost in other gameplay files
+        self.role_cost = goalie_cost()
+
+    def compute_props(self):
+        pass
+
+    def create_request(self, **kwargs) -> role.RoleRequest:
+        """Creates a sane default RoleRequest.
+        :return: A list of size 1 of a sane default RoleRequest.
+        """
+        pass
+
+    def get_requests(self, world_state: rc.WorldState,
+                     props) -> List[tactic.RoleRequests]:
+        """
+        :return: A list of role requests for move skills needed
+        """
+
+        if world_state and world_state.ball.visible:
+            # update move skill
+            self.move_se.skill.target_point = get_goalie_pt(world_state)
+            self.move_se.skill.face_point = world_state.ball.pos
+
+        # create RoleRequest for each SkillEntry
+        role_requests = {}
+        role_requests[self.move_se] = [role.RoleRequest(role.Priority.HIGH, False, self.role_cost)]
+        return role_requests
+
+    def tick(self,
+             role_results: tactic.RoleResults) -> List[tactic.SkillEntry]:
+        """
+        :return: A list of skills depending on which roles are filled
+        """
+
+        # create list of skills based on if RoleResult exists for SkillEntry
+        skills = []
+        if role_results[self.move_se][0]:
+            skills.append(self.move_se)
+
+        return skills
+
+    def is_done(self, world_state):
+        """
+        :return boolean indicating if tactic is done
+        """
+        return self.move_se.skill.is_done(world_state)

From c988e93d34e9dc2affc0435805594a0a8816d660 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Wed, 16 Jun 2021 22:24:48 -0400
Subject: [PATCH 07/28] fix capture skill typo

---
 rj_gameplay/rj_gameplay/skill/capture.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/skill/capture.py b/rj_gameplay/rj_gameplay/skill/capture.py
index 5be01ab2526..66512ec41e6 100644
--- a/rj_gameplay/rj_gameplay/skill/capture.py
+++ b/rj_gameplay/rj_gameplay/skill/capture.py
@@ -22,9 +22,9 @@ class ICapture(skill.ISkill, ABC):
 """
 class Capture(ICapture):
     
-    def __init__(self):
-        self.robot: rc.Robot = None
-        self.__name__ = 'Capture'
+    def __init__(self, robot: rc.Robot = None):
+        self.robot = robot
+        self.__name__ = 'Capture skill'
         self.capture = capture.Capture()
         self.capture_behavior = ActionBehavior('Capture', self.capture, self.robot)
         self.root = self.capture_behavior

From 59efb8e929e408810be898ae8217904e9fd09ea8 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Wed, 16 Jun 2021 22:43:15 -0400
Subject: [PATCH 08/28] add intercept skill/action

---
 rj_gameplay/rj_gameplay/action/intercept.py | 34 ++++++++++++++++
 rj_gameplay/rj_gameplay/skill/intercept.py  | 45 +++++++++++++++++++++
 2 files changed, 79 insertions(+)
 create mode 100644 rj_gameplay/rj_gameplay/action/intercept.py
 create mode 100644 rj_gameplay/rj_gameplay/skill/intercept.py

diff --git a/rj_gameplay/rj_gameplay/action/intercept.py b/rj_gameplay/rj_gameplay/action/intercept.py
new file mode 100644
index 00000000000..4df05644a77
--- /dev/null
+++ b/rj_gameplay/rj_gameplay/action/intercept.py
@@ -0,0 +1,34 @@
+"""This module contains the interface and action for intercept."""
+
+from abc import ABC, abstractmethod
+
+import stp.role as role
+import stp.action as action
+import stp.rc as rc
+import numpy as np
+from rj_msgs.msg import RobotIntent, InterceptMotionCommand
+from rj_geometry_msgs.msg import Point
+
+class Intercept(action.IAction):
+    """Intercept action
+    """
+
+    def __init__(self, robot_id: int = None, target_point: np.ndarray = None):
+        self.robot_id = robot_id
+        self.target_point = target_point
+
+
+    def tick(self, intent: RobotIntent) -> None:
+        intercept_command = InterceptMotionCommand()
+        # TODO: numpy to Point conv
+        intercept_command.target = Point(x=self.target_point[0], y=self.target_point[1])
+        intent.motion_command.intercept_command = [intercept_command] 
+        intent.dribbler_speed = 1.0
+        intent.is_active = True
+
+        return intent
+
+    def is_done(self, world_state) -> bool:
+        if world_state.our_robots[self.robot_id].has_ball_sense:
+            return True
+        return False
diff --git a/rj_gameplay/rj_gameplay/skill/intercept.py b/rj_gameplay/rj_gameplay/skill/intercept.py
new file mode 100644
index 00000000000..6c406c5389b
--- /dev/null
+++ b/rj_gameplay/rj_gameplay/skill/intercept.py
@@ -0,0 +1,45 @@
+from abc import ABC, abstractmethod
+
+import rj_gameplay.eval as eval
+import argparse
+import py_trees
+import sys
+import time
+
+import stp.skill as skill
+import stp.role as role
+import stp.action as action
+from rj_gameplay.action import intercept 
+from stp.skill.action_behavior import ActionBehavior
+import stp.rc as rc
+import numpy as np
+
+class IIntercept(skill.ISkill, ABC):
+    ...
+
+
+"""
+A skill version of intercept so that actions don't have to be called in tactics
+"""
+# TODO: discuss collapsing skills/actions
+class Intercept(IIntercept):
+    
+    def __init__(self, robot: rc.Robot=None, target_point: np.ndarray=np.array([0.0, 0.0])):
+        self.robot = robot
+        self.target_point = target_point
+
+        self.__name__ = 'Intercept Skill'
+        if self.robot is not None:
+            self.intercept = intercept.Intercept(self.robot.id, target_point)
+        else:
+            self.intercept = intercept.Intercept(None, target_point)
+        self.intercept_behavior = ActionBehavior('Intercept', self.intercept, self.robot)
+        self.root = self.intercept_behavior
+        self.root.setup_with_descendants()
+
+    def tick(self, robot: rc.Robot, world_state: rc.WorldState):
+        self.robot = robot
+        return self.root.tick_once(robot, world_state)
+
+    def is_done(self, world_state) -> bool:
+    	return self.intercept.is_done(world_state)

From 8928a483c83c3f4f892c7507cdf416d4e132aea0 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Wed, 16 Jun 2021 23:15:08 -0400
Subject: [PATCH 09/28] goalie tracks ball, does not intercept yet

---
 .../rj_gameplay/tactic/goalie_tactic.py       | 52 ++++++++++++++-----
 rj_gameplay/rj_gameplay/tactic/wall_tactic.py |  6 ++-
 2 files changed, 44 insertions(+), 14 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
index 1e9b115db16..ee5bb28ac14 100644
--- a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -11,12 +11,13 @@
 
 import rj_gameplay.eval
 import rj_gameplay.skill as skills
-from rj_gameplay.skill import move
+from rj_gameplay.skill import move, intercept
 import stp.skill as skill
 import numpy as np
 # TODO: replace w/ global param server
 from stp.utils.constants import RobotConstants, BallConstants
 import stp.global_parameters as global_parameters
+from stp.local_parameters import Param
 
 
 class goalie_cost(role.CostFn):
@@ -34,12 +35,12 @@ def get_goalie_pt(world_state: rc.WorldState) -> np.ndarray:
     """Finds point for goalie to best intercept a shot.
     :return numpy point
     """
-    # TODO: param server this const
     # TODO: param server any constant from stp/utils/constants.py (this includes BallConstants)
     ball_pt = world_state.ball.pos
     goal_pt = world_state.field.our_goal_loc
 
-    PCT_TO_BALL = 0.50
+    # TODO: param server this const
+    PCT_TO_BALL = 0.10
 
     # get direction vec to ball
     dir_vec = (ball_pt - goal_pt) / np.linalg.norm(ball_pt - goal_pt)
@@ -55,7 +56,8 @@ def __init__(self):
 
         # create move SkillEntry
         self.move_se = tactic.SkillEntry(move.Move())
-        # TODO: intercept skill
+
+        self.intercept_se = tactic.SkillEntry(intercept.Intercept())
 
         # TODO: rename cost_list to role_cost in other gameplay files
         self.role_cost = goalie_cost()
@@ -75,14 +77,36 @@ def get_requests(self, world_state: rc.WorldState,
         :return: A list of role requests for move skills needed
         """
 
-        if world_state and world_state.ball.visible:
-            # update move skill
-            self.move_se.skill.target_point = get_goalie_pt(world_state)
-            self.move_se.skill.face_point = world_state.ball.pos
+        # TODO: this const is copy-pasted from wall_tactic 
+        # put into common param file: https://www.geeksforgeeks.org/global-keyword-in-python/
+
+        # dist is slightly greater than penalty box bounds
+        box_w = world_state.field.penalty_long_dist_m
+        box_h = world_state.field.penalty_short_dist_m
+        line_w = world_state.field.line_width_m
+        MIN_WALL_RAD = RobotConstants.RADIUS + line_w + np.hypot(box_w / 2, box_h)
 
-        # create RoleRequest for each SkillEntry
         role_requests = {}
-        role_requests[self.move_se] = [role.RoleRequest(role.Priority.HIGH, False, self.role_cost)]
+        if world_state and world_state.ball.visible:
+            ball_to_goal_dist = np.linalg.norm(world_state.field.our_goal_loc - world_state.ball.pos)
+            if ball_to_goal_dist < MIN_WALL_RAD:
+                print("INTERCEPT"*80)
+                # intercept when inside wall
+
+                # update intercept skill
+                self.intercept_se.skill.target_point = world_state.ball.pos
+
+                # create RoleRequest for each SkillEntry
+                role_requests[self.intercept_se] = [role.RoleRequest(role.Priority.HIGH, False, self.role_cost)]
+            else:
+                # else, track ball normally
+
+                # update move skill
+                self.move_se.skill.target_point = get_goalie_pt(world_state)
+                self.move_se.skill.face_point = world_state.ball.pos
+
+                # create RoleRequest for each SkillEntry
+                role_requests[self.move_se] = [role.RoleRequest(role.Priority.HIGH, False, self.role_cost)]
         return role_requests
 
     def tick(self,
@@ -93,8 +117,12 @@ def tick(self,
 
         # create list of skills based on if RoleResult exists for SkillEntry
         skills = []
-        if role_results[self.move_se][0]:
-            skills.append(self.move_se)
+        if role_results[self.move_se]:
+            if role_results[self.move_se][0]:
+                skills.append(self.move_se)
+        elif role_results[self.intercept_se]:
+            if role_results[self.intercept_se][0]:
+                skills.append(self.intercept_se)
 
         return skills
 
diff --git a/rj_gameplay/rj_gameplay/tactic/wall_tactic.py b/rj_gameplay/rj_gameplay/tactic/wall_tactic.py
index 1de7929332a..6ccfe2d09b4 100644
--- a/rj_gameplay/rj_gameplay/tactic/wall_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/wall_tactic.py
@@ -18,6 +18,7 @@
 from stp.utils.constants import RobotConstants, BallConstants
 import stp.global_parameters as global_parameters
 
+MIN_WALL_RAD = None
 
 class wall_cost(role.CostFn):
     """Cost function for role request.
@@ -41,6 +42,7 @@ def __call__(self, robot: rc.Robot, prev_result: Optional["RoleResult"],
 
 def find_wall_pts(num_wallers: int,
                   world_state: rc.WorldState) -> List[np.ndarray]:
+    global MIN_WALL_RAD
     """Calculates num_wallers points to form a wall between the ball and goal.
     :return list of wall_pts (as numpy arrays)
     """
@@ -55,14 +57,14 @@ def find_wall_pts(num_wallers: int,
     box_w = world_state.field.penalty_long_dist_m
     box_h = world_state.field.penalty_short_dist_m
     line_w = world_state.field.line_width_m
-    DIST_FROM_DEF = RobotConstants.RADIUS + line_w + np.hypot(box_w / 2, box_h)
+    MIN_WALL_RAD = RobotConstants.RADIUS + line_w + np.hypot(box_w / 2, box_h)
 
     # get direction vec
     dir_vec = (ball_pt - goal_pt) / np.linalg.norm(ball_pt - goal_pt)
     wall_vec = np.array([dir_vec[1], -dir_vec[0]])
 
     # find mid_pt
-    mid_pt = goal_pt + (dir_vec * DIST_FROM_DEF)
+    mid_pt = goal_pt + (dir_vec * MIN_WALL_RAD)
     wall_pts = [mid_pt]
 
     # set wall points in middle out pattern, given wall dir vector and WALL_SPACING constant

From 2cfd6dfe1235767d011ede87cef41be577a9deeb Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Thu, 17 Jun 2021 12:00:30 -0400
Subject: [PATCH 10/28] minor merge conflict

---
 rj_gameplay/rj_gameplay/tactic/goalie_tactic.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
index ee5bb28ac14..0781c186d06 100644
--- a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -21,7 +21,7 @@
 
 
 class goalie_cost(role.CostFn):
-    """Cost function for role request. Want only the goalie to be selected.
+    """Cost function for role request. Want only the designated goalie to be selected.
     """
     def __call__(self, robot: rc.Robot, prev_result: Optional["RoleResult"],
                  world_state: rc.WorldState) -> float:

From 60191cff04e9ee2f5fdf4ea02aee63de3cd5cbef Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Thu, 17 Jun 2021 12:00:49 -0400
Subject: [PATCH 11/28] fix race cond goalie msg

---
 rj_gameplay/rj_gameplay/gameplay_node.py | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/rj_gameplay/rj_gameplay/gameplay_node.py b/rj_gameplay/rj_gameplay/gameplay_node.py
index 744d30923d2..d72021a6a94 100644
--- a/rj_gameplay/rj_gameplay/gameplay_node.py
+++ b/rj_gameplay/rj_gameplay/gameplay_node.py
@@ -57,6 +57,7 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
         self.world_state = world_state
         self.partial_world_state: conv.PartialWorldState = None
         self.game_info: rc.GameInfo = None
+        self.goalie_id = None
         self.field: rc.Field = None
         self.robot_statuses: List[conv.RobotStatus] = [conv.RobotStatus()]*NUM_ROBOTS*2
 
@@ -94,6 +95,8 @@ def create_game_info(self, msg: msg.GameState) -> None:
         """
         if msg is not None:
             self.game_info = conv.gamestate_to_gameinfo(msg)
+        if self.goalie_id is not None:
+            self.game_info.set_goalie_id(msg.goalie_id)
 
     def create_field(self, msg: msg.FieldDimensions) -> None:
         """
@@ -107,6 +110,7 @@ def set_goalie_id(self, msg: msg.Goalie) -> None:
         Set goalie id based on goalie msg
         """
         if msg is not None and self.game_info is not None:
+            self.goalie_id = msg.goalie_id
             self.game_info.set_goalie_id(msg.goalie_id)
 
     def get_world_state(self) -> rc.WorldState:

From 80fa15f0c0d03acb96b0a6d78bfbacfce1acea0d Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Thu, 17 Jun 2021 12:05:43 -0400
Subject: [PATCH 12/28] fix intercept skill tick

---
 rj_gameplay/rj_gameplay/skill/intercept.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/rj_gameplay/rj_gameplay/skill/intercept.py b/rj_gameplay/rj_gameplay/skill/intercept.py
index 6c406c5389b..3f2e8f5e973 100644
--- a/rj_gameplay/rj_gameplay/skill/intercept.py
+++ b/rj_gameplay/rj_gameplay/skill/intercept.py
@@ -39,7 +39,9 @@ def __init__(self, robot: rc.Robot=None, target_point: np.ndarray=np.array([0.0,
 
     def tick(self, robot: rc.Robot, world_state: rc.WorldState):
         self.robot = robot
-        return self.root.tick_once(robot, world_state)
+        self.intercept.robot_id = self.robot.id
+        self.intercept.target_point = self.target_point
+        return self.root.tick_once(self.robot, world_state)
 
     def is_done(self, world_state) -> bool:
     	return self.intercept.is_done(world_state)

From cc824bc96cc5881261a408c3b6ecae79551a46a6 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Thu, 17 Jun 2021 12:10:13 -0400
Subject: [PATCH 13/28] actually fix goalie id race cond

---
 rj_gameplay/rj_gameplay/gameplay_node.py | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/gameplay_node.py b/rj_gameplay/rj_gameplay/gameplay_node.py
index d72021a6a94..370fcc31267 100644
--- a/rj_gameplay/rj_gameplay/gameplay_node.py
+++ b/rj_gameplay/rj_gameplay/gameplay_node.py
@@ -96,7 +96,7 @@ def create_game_info(self, msg: msg.GameState) -> None:
         if msg is not None:
             self.game_info = conv.gamestate_to_gameinfo(msg)
         if self.goalie_id is not None:
-            self.game_info.set_goalie_id(msg.goalie_id)
+            self.game_info.set_goalie_id(self.goalie_id)
 
     def create_field(self, msg: msg.FieldDimensions) -> None:
         """
@@ -109,9 +109,10 @@ def set_goalie_id(self, msg: msg.Goalie) -> None:
         """
         Set goalie id based on goalie msg
         """
-        if msg is not None and self.game_info is not None:
+        if msg is not None:
             self.goalie_id = msg.goalie_id
-            self.game_info.set_goalie_id(msg.goalie_id)
+            if self.game_info is not None:
+                self.game_info.set_goalie_id(self.goalie_id)
 
     def get_world_state(self) -> rc.WorldState:
         """

From fa03decf4525115e77962226087ef85dd951d077 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Thu, 17 Jun 2021 13:11:37 -0400
Subject: [PATCH 14/28] switched to settle cmd

---
 rj_gameplay/rj_gameplay/action/intercept.py | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/rj_gameplay/rj_gameplay/action/intercept.py b/rj_gameplay/rj_gameplay/action/intercept.py
index 4df05644a77..0c811c08eda 100644
--- a/rj_gameplay/rj_gameplay/action/intercept.py
+++ b/rj_gameplay/rj_gameplay/action/intercept.py
@@ -6,7 +6,7 @@
 import stp.action as action
 import stp.rc as rc
 import numpy as np
-from rj_msgs.msg import RobotIntent, InterceptMotionCommand
+from rj_msgs.msg import RobotIntent, InterceptMotionCommand, SettleMotionCommand
 from rj_geometry_msgs.msg import Point
 
 class Intercept(action.IAction):
@@ -19,10 +19,15 @@ def __init__(self, robot_id: int = None, target_point: np.ndarray = None):
 
 
     def tick(self, intent: RobotIntent) -> None:
+        """
+        # TODO: use this when intercept is fixed
         intercept_command = InterceptMotionCommand()
         # TODO: numpy to Point conv
         intercept_command.target = Point(x=self.target_point[0], y=self.target_point[1])
         intent.motion_command.intercept_command = [intercept_command] 
+        """
+        settle_command = SettleMotionCommand()
+        intent.motion_command.settle_command = [settle_command] 
         intent.dribbler_speed = 1.0
         intent.is_active = True
 

From 47e8ab0888b9eb8de73a51095ab8d3fb6df773fd Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Thu, 17 Jun 2021 13:16:20 -0400
Subject: [PATCH 15/28] rm debug print

---
 rj_gameplay/rj_gameplay/tactic/goalie_tactic.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
index 0781c186d06..be5782ea537 100644
--- a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -90,7 +90,6 @@ def get_requests(self, world_state: rc.WorldState,
         if world_state and world_state.ball.visible:
             ball_to_goal_dist = np.linalg.norm(world_state.field.our_goal_loc - world_state.ball.pos)
             if ball_to_goal_dist < MIN_WALL_RAD:
-                print("INTERCEPT"*80)
                 # intercept when inside wall
 
                 # update intercept skill

From d52f283b0c9b5086d8f36d3acd074f18673c39e9 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Thu, 17 Jun 2021 13:48:07 -0400
Subject: [PATCH 16/28] obstacle node change, directed by Kyle

---
 rj_gameplay/rj_gameplay/gameplay_node.py    | 10 +++++-----
 soccer/src/soccer/planning/planner_node.cpp |  2 +-
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/gameplay_node.py b/rj_gameplay/rj_gameplay/gameplay_node.py
index 370fcc31267..fed709010e3 100644
--- a/rj_gameplay/rj_gameplay/gameplay_node.py
+++ b/rj_gameplay/rj_gameplay/gameplay_node.py
@@ -66,7 +66,7 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
         local_parameters.register_parameters(self)
 
         # publish global obstacles
-        self.global_obstacles_pub = self.create_publisher(geo_msg.ShapeSet, '/planning/global_obstacles', 10)
+        self.goal_zone_obstacles_pub = self.create_publisher(geo_msg.ShapeSet, '/planning/goal_zone_obstacles', 10)
 
         timer_period = 1/60 #seconds
         self.timer = self.create_timer(timer_period, self.gameplay_tick)
@@ -151,10 +151,10 @@ def gameplay_tick(self) -> None:
             top_right = geo_msg.Point(x=-self.field.penalty_long_dist_m/2 - self.field.line_width_m, y=self.field.length_m - self.field.penalty_short_dist_m)
             their_penalty.pt = [bot_left, top_right]
 
-            # publish Rect shape to global_obstacles topic
-            global_obstacles = geo_msg.ShapeSet()
-            global_obstacles.rectangles = [our_penalty, their_penalty]
-            self.global_obstacles_pub.publish(global_obstacles)
+            # publish Rect shape to goal_zone_obstacles topic
+            goal_zone_obstacles = geo_msg.ShapeSet()
+            goal_zone_obstacles.rectangles = [our_penalty, their_penalty]
+            self.goal_zone_obstacles_pub.publish(goal_zone_obstacles)
         else:
             self.get_logger().warn("World state was none!")
 
diff --git a/soccer/src/soccer/planning/planner_node.cpp b/soccer/src/soccer/planning/planner_node.cpp
index 9c9288a979a..e3fc9a9594d 100644
--- a/soccer/src/soccer/planning/planner_node.cpp
+++ b/soccer/src/soccer/planning/planner_node.cpp
@@ -98,7 +98,7 @@ PlanRequest PlannerForRobot::make_request(const RobotIntent& intent) {
     return PlanRequest{start,
                        intent.motion_command,
                        constraints,
-                       global_obstacles_,
+                       obstacles,
                        intent.local_obstacles,
                        planned_trajectories,
                        static_cast<unsigned int>(robot_id_),

From 655e7b86930db4e7ee6dcad56be81591f037b11f Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Fri, 18 Jun 2021 20:25:03 -0400
Subject: [PATCH 17/28] temp debug prints back

---
 rj_gameplay/rj_gameplay/gameplay_node.py      | 20 ++++++++++++-------
 .../rj_gameplay/tactic/goalie_tactic.py       |  2 ++
 2 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/gameplay_node.py b/rj_gameplay/rj_gameplay/gameplay_node.py
index fed709010e3..5d9c31aa88a 100644
--- a/rj_gameplay/rj_gameplay/gameplay_node.py
+++ b/rj_gameplay/rj_gameplay/gameplay_node.py
@@ -30,7 +30,7 @@ def select(self, world_state: rc.WorldState) -> Tuple[situation.ISituation, stp.
 
 class GameplayNode(Node):
     """
-    A node which subscribes to the world_state,  game state, robot status, and field topics and converts the messages to python types.
+    A node which subscribes to the world_state, game state, robot status, and field topics and converts the messages to python types.
     """
 
     def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional[rc.WorldState] = None) -> None:
@@ -39,7 +39,7 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
         self.world_state_sub = self.create_subscription(msg.WorldState, '/vision_filter/world_state', self.create_partial_world_state, 10)
         self.field_dimensions = self.create_subscription(msg.FieldDimensions, '/config/field_dimensions', self.create_field, 10)
         self.game_info = self.create_subscription(msg.GameState, '/referee/game_state', self.create_game_info, 10)
-        self.goalie_id_sub = self.create_subscription(msg.Goalie, '/referee/our_goalie', self.set_goalie_id, 10)
+        self.goalie_id_sub = self.create_subscription(msg.Goalie, '/referee/our_goalie', self.create_goalie_id, 10)
 
         self.robot_state_subs = [None] * NUM_ROBOTS
         self.robot_intent_pubs = [None] * NUM_ROBOTS
@@ -95,8 +95,13 @@ def create_game_info(self, msg: msg.GameState) -> None:
         """
         if msg is not None:
             self.game_info = conv.gamestate_to_gameinfo(msg)
+            # self.game_info.set_goalie_id(4)
+        """
         if self.goalie_id is not None:
+            print("set game info")
+            print(self.goalie_id)
             self.game_info.set_goalie_id(self.goalie_id)
+        """
 
     def create_field(self, msg: msg.FieldDimensions) -> None:
         """
@@ -105,14 +110,15 @@ def create_field(self, msg: msg.FieldDimensions) -> None:
         if msg is not None:
             self.field = conv.field_msg_to_field(msg)
 
-    def set_goalie_id(self, msg: msg.Goalie) -> None:
+    def create_goalie_id(self, msg: msg.Goalie) -> None:
         """
-        Set goalie id based on goalie msg
+        Set game_info's goalie_id based on goalie msg
         """
-        if msg is not None:
+        print("msg")
+        print(msg)
+        if msg is not None and self.game_info is not None:
             self.goalie_id = msg.goalie_id
-            if self.game_info is not None:
-                self.game_info.set_goalie_id(self.goalie_id)
+            self.game_info.set_goalie_id(msg.goalie_id)
 
     def get_world_state(self) -> rc.WorldState:
         """
diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
index be5782ea537..b4e5f1cf784 100644
--- a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -27,6 +27,8 @@ def __call__(self, robot: rc.Robot, prev_result: Optional["RoleResult"],
                  world_state: rc.WorldState) -> float:
 
         if world_state.game_info is not None:
+            print(world_state.game_info.our_restart)
+            print(world_state.game_info.goalie_id)
             if robot.id == world_state.game_info.goalie_id:
                 return -1.0
         return 999.0

From 5a7df09b2c892088d37f36a79ecc4e0bb35f61e0 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Fri, 18 Jun 2021 20:34:56 -0400
Subject: [PATCH 18/28] goalie switched to receive

---
 rj_gameplay/rj_gameplay/gameplay_node.py      |  2 +-
 .../rj_gameplay/tactic/goalie_tactic.py       | 30 +++++++++----------
 2 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/gameplay_node.py b/rj_gameplay/rj_gameplay/gameplay_node.py
index d12bdb75afd..25c17e2f629 100644
--- a/rj_gameplay/rj_gameplay/gameplay_node.py
+++ b/rj_gameplay/rj_gameplay/gameplay_node.py
@@ -12,7 +12,7 @@
 from stp.global_parameters import GlobalParameterClient
 import numpy as np
 from rj_gameplay.action.move import Move
-from rj_gameplay.play import line_up, passing_tactic_play
+from rj_gameplay.play import basic_defense
 from typing import List, Optional, Tuple
 
 NUM_ROBOTS = 16
diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
index b4e5f1cf784..c8787ab16c6 100644
--- a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -11,7 +11,7 @@
 
 import rj_gameplay.eval
 import rj_gameplay.skill as skills
-from rj_gameplay.skill import move, intercept
+from rj_gameplay.skill import move, receive #, intercept
 import stp.skill as skill
 import numpy as np
 # TODO: replace w/ global param server
@@ -29,12 +29,13 @@ def __call__(self, robot: rc.Robot, prev_result: Optional["RoleResult"],
         if world_state.game_info is not None:
             print(world_state.game_info.our_restart)
             print(world_state.game_info.goalie_id)
-            if robot.id == world_state.game_info.goalie_id:
+            if robot.id == 0:
+            # if robot.id == world_state.game_info.goalie_id:
                 return -1.0
         return 999.0
 
 def get_goalie_pt(world_state: rc.WorldState) -> np.ndarray:
-    """Finds point for goalie to best intercept a shot.
+    """Finds point for goalie to best be in to block a shot.
     :return numpy point
     """
     # TODO: param server any constant from stp/utils/constants.py (this includes BallConstants)
@@ -59,7 +60,7 @@ def __init__(self):
         # create move SkillEntry
         self.move_se = tactic.SkillEntry(move.Move())
 
-        self.intercept_se = tactic.SkillEntry(intercept.Intercept())
+        self.receive_se = tactic.SkillEntry(receive.Receive())
 
         # TODO: rename cost_list to role_cost in other gameplay files
         self.role_cost = goalie_cost()
@@ -92,13 +93,10 @@ def get_requests(self, world_state: rc.WorldState,
         if world_state and world_state.ball.visible:
             ball_to_goal_dist = np.linalg.norm(world_state.field.our_goal_loc - world_state.ball.pos)
             if ball_to_goal_dist < MIN_WALL_RAD:
-                # intercept when inside wall
-
-                # update intercept skill
-                self.intercept_se.skill.target_point = world_state.ball.pos
+                # move to block when ball inside wall
 
                 # create RoleRequest for each SkillEntry
-                role_requests[self.intercept_se] = [role.RoleRequest(role.Priority.HIGH, False, self.role_cost)]
+                role_requests[self.receive_se] = [role.RoleRequest(role.Priority.HIGH, False, self.role_cost)]
             else:
                 # else, track ball normally
 
@@ -118,12 +116,14 @@ def tick(self,
 
         # create list of skills based on if RoleResult exists for SkillEntry
         skills = []
-        if role_results[self.move_se]:
-            if role_results[self.move_se][0]:
-                skills.append(self.move_se)
-        elif role_results[self.intercept_se]:
-            if role_results[self.intercept_se][0]:
-                skills.append(self.intercept_se)
+
+        move_result = role_results[self.move_se]
+        if move_result and move_result[0].is_filled():
+            skills.append(self.move_se)
+
+        receive_result = role_results[self.receive_se]
+        if receive_result and receive_result[0].is_filled():
+            skills.append(self.receive_se)
 
         return skills
 

From f489e55eff5c29c4a366e4b4927f152db61a6f96 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Fri, 18 Jun 2021 22:39:50 -0400
Subject: [PATCH 19/28] add intercept calculations with move command

---
 .../rj_gameplay/tactic/goalie_tactic.py       | 75 ++++++++++++-------
 1 file changed, 47 insertions(+), 28 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
index c8787ab16c6..dfa12985c8f 100644
--- a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -19,6 +19,9 @@
 import stp.global_parameters as global_parameters
 from stp.local_parameters import Param
 
+# TODO: param server this const
+MIN_WALL_RAD = 0
+GOALIE_PCT_TO_BALL = 0.15
 
 class goalie_cost(role.CostFn):
     """Cost function for role request. Want only the designated goalie to be selected.
@@ -27,8 +30,12 @@ def __call__(self, robot: rc.Robot, prev_result: Optional["RoleResult"],
                  world_state: rc.WorldState) -> float:
 
         if world_state.game_info is not None:
-            print(world_state.game_info.our_restart)
-            print(world_state.game_info.goalie_id)
+            # TODO: clear debug prints
+            # print(world_state.game_info.our_restart)
+            # print(world_state.game_info.goalie_id)
+
+            # this is a hacky way of doing goalie, until sub is fixed
+            # TODO: fix goalie_id sub in gameplay node
             if robot.id == 0:
             # if robot.id == world_state.game_info.goalie_id:
                 return -1.0
@@ -42,17 +49,21 @@ def get_goalie_pt(world_state: rc.WorldState) -> np.ndarray:
     ball_pt = world_state.ball.pos
     goal_pt = world_state.field.our_goal_loc
 
-    # TODO: param server this const
-    PCT_TO_BALL = 0.10
-
-    # get direction vec to ball
     dir_vec = (ball_pt - goal_pt) / np.linalg.norm(ball_pt - goal_pt)
-    wall_vec = np.array([dir_vec[1], -dir_vec[0]])
-
-    # find and return pt
-    mid_pt = goal_pt + (dir_vec * PCT_TO_BALL * np.linalg.norm(ball_pt - goal_pt))
+    # get in-between ball and goal, staying behind wall
+    dist_from_goal = min(GOALIE_PCT_TO_BALL * np.linalg.norm(ball_pt - goal_pt), MIN_WALL_RAD - RobotConstants.RADIUS * 2.1)
+    mid_pt = goal_pt + (dir_vec * dist_from_goal)
     return mid_pt
 
+# TODO: replace with intercept
+def get_block_pt(world_state: rc.WorldState, my_pos: np.ndarray) -> np.ndarray:
+    pos = world_state.ball.pos
+    vel = world_state.ball.vel
+
+    block_pt = np.array([(my_pos[1] - pos[1]) / vel[1] * vel[0] + pos[0], my_pos[1]])
+
+    return block_pt
+
 
 class GoalieTactic(tactic.ITactic):
     def __init__(self):
@@ -60,6 +71,7 @@ def __init__(self):
         # create move SkillEntry
         self.move_se = tactic.SkillEntry(move.Move())
 
+        # TODO: replace w/ intercept
         self.receive_se = tactic.SkillEntry(receive.Receive())
 
         # TODO: rename cost_list to role_cost in other gameplay files
@@ -76,11 +88,12 @@ def create_request(self, **kwargs) -> role.RoleRequest:
 
     def get_requests(self, world_state: rc.WorldState,
                      props) -> List[tactic.RoleRequests]:
+        global MIN_WALL_RAD
         """
         :return: A list of role requests for move skills needed
         """
 
-        # TODO: this const is copy-pasted from wall_tactic 
+        # TODO: this calculation is copy-pasted from wall_tactic 
         # put into common param file: https://www.geeksforgeeks.org/global-keyword-in-python/
 
         # dist is slightly greater than penalty box bounds
@@ -91,21 +104,25 @@ def get_requests(self, world_state: rc.WorldState,
 
         role_requests = {}
         if world_state and world_state.ball.visible:
-            ball_to_goal_dist = np.linalg.norm(world_state.field.our_goal_loc - world_state.ball.pos)
-            if ball_to_goal_dist < MIN_WALL_RAD:
-                # move to block when ball inside wall
+            ball_speed = np.linalg.norm(world_state.ball.vel)
+            ball_dist = np.linalg.norm(world_state.field.our_goal_loc - world_state.ball.pos)
 
-                # create RoleRequest for each SkillEntry
-                role_requests[self.receive_se] = [role.RoleRequest(role.Priority.HIGH, False, self.role_cost)]
+            if ball_speed < 1.0 and ball_dist < MIN_WALL_RAD - RobotConstants.RADIUS * 2.1:
+                # if ball is slow and inside goalie box, collect it
+                role_requests[self.receive_se] = [role.RoleRequest(role.Priority.HIGH, True, self.role_cost)]
             else:
-                # else, track ball normally
+                ball_to_goal_time = ball_dist / ball_speed
+                if ball_speed > 0 and ball_to_goal_time < 2:
+                    # if ball is moving and coming at goal, move laterally to block ball
+                    self.move_se.skill.target_point = get_block_pt(world_state, get_goalie_pt(world_state))
+                    self.move_se.skill.face_point = world_state.ball.pos
+                    role_requests[self.move_se] = [role.RoleRequest(role.Priority.HIGH, True, self.role_cost)]
+                else:
+                    # else, track ball normally
+                    self.move_se.skill.target_point = get_goalie_pt(world_state)
+                    self.move_se.skill.face_point = world_state.ball.pos
+                    role_requests[self.move_se] = [role.RoleRequest(role.Priority.HIGH, True, self.role_cost)]
 
-                # update move skill
-                self.move_se.skill.target_point = get_goalie_pt(world_state)
-                self.move_se.skill.face_point = world_state.ball.pos
-
-                # create RoleRequest for each SkillEntry
-                role_requests[self.move_se] = [role.RoleRequest(role.Priority.HIGH, False, self.role_cost)]
         return role_requests
 
     def tick(self,
@@ -120,10 +137,11 @@ def tick(self,
         move_result = role_results[self.move_se]
         if move_result and move_result[0].is_filled():
             skills.append(self.move_se)
-
-        receive_result = role_results[self.receive_se]
-        if receive_result and receive_result[0].is_filled():
-            skills.append(self.receive_se)
+        else:
+            # move first, then receive
+            receive_result = role_results[self.receive_se]
+            if receive_result and receive_result[0].is_filled():
+                skills.append(self.receive_se)
 
         return skills
 
@@ -131,4 +149,5 @@ def is_done(self, world_state):
         """
         :return boolean indicating if tactic is done
         """
-        return self.move_se.skill.is_done(world_state)
+        # goalie tactic always active
+        return False

From f33cdef30244d5483701d3de1068003fb300f021 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Fri, 18 Jun 2021 22:46:40 -0400
Subject: [PATCH 20/28] pretty-lines

---
 rj_gameplay/rj_gameplay/action/intercept.py   |  5 +--
 rj_gameplay/rj_gameplay/gameplay_node.py      |  9 +++-
 rj_gameplay/rj_gameplay/skill/intercept.py    | 15 ++++---
 .../rj_gameplay/tactic/goalie_tactic.py       | 42 +++++++++++++------
 rj_gameplay/rj_gameplay/tactic/wall_tactic.py |  1 +
 rj_gameplay/stp/rc.py                         |  6 ++-
 .../stp/utils/world_state_converter.py        |  6 +--
 7 files changed, 57 insertions(+), 27 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/action/intercept.py b/rj_gameplay/rj_gameplay/action/intercept.py
index 0c811c08eda..b2152b58912 100644
--- a/rj_gameplay/rj_gameplay/action/intercept.py
+++ b/rj_gameplay/rj_gameplay/action/intercept.py
@@ -9,15 +9,14 @@
 from rj_msgs.msg import RobotIntent, InterceptMotionCommand, SettleMotionCommand
 from rj_geometry_msgs.msg import Point
 
+
 class Intercept(action.IAction):
     """Intercept action
     """
-
     def __init__(self, robot_id: int = None, target_point: np.ndarray = None):
         self.robot_id = robot_id
         self.target_point = target_point
 
-
     def tick(self, intent: RobotIntent) -> None:
         """
         # TODO: use this when intercept is fixed
@@ -27,7 +26,7 @@ def tick(self, intent: RobotIntent) -> None:
         intent.motion_command.intercept_command = [intercept_command] 
         """
         settle_command = SettleMotionCommand()
-        intent.motion_command.settle_command = [settle_command] 
+        intent.motion_command.settle_command = [settle_command]
         intent.dribbler_speed = 1.0
         intent.is_active = True
 
diff --git a/rj_gameplay/rj_gameplay/gameplay_node.py b/rj_gameplay/rj_gameplay/gameplay_node.py
index 25c17e2f629..df9aaabc90c 100644
--- a/rj_gameplay/rj_gameplay/gameplay_node.py
+++ b/rj_gameplay/rj_gameplay/gameplay_node.py
@@ -27,6 +27,7 @@ class TestPlaySelector(situation.IPlaySelector):
     def select(self, world_state: rc.WorldState) -> Tuple[situation.ISituation, stp.play.IPlay]:
         return (None, basic_defense.BasicDefense())
 
+
 class GameplayNode(Node):
     """
     A node which subscribes to the world_state, game state, robot status, and field topics and converts the messages to python types.
@@ -38,7 +39,10 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
         self.world_state_sub = self.create_subscription(msg.WorldState, '/vision_filter/world_state', self.create_partial_world_state, 10)
         self.field_dimensions = self.create_subscription(msg.FieldDimensions, '/config/field_dimensions', self.create_field, 10)
         self.game_info = self.create_subscription(msg.GameState, '/referee/game_state', self.create_game_info, 10)
-        self.goalie_id_sub = self.create_subscription(msg.Goalie, '/referee/our_goalie', self.create_goalie_id, 10)
+        self.goalie_id_sub = self.create_subscription(msg.Goalie,
+                                                      '/referee/our_goalie',
+                                                      self.create_goalie_id,
+                                                      10)
 
         self.robot_state_subs = [None] * NUM_ROBOTS
         self.robot_intent_pubs = [None] * NUM_ROBOTS
@@ -65,7 +69,8 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
         local_parameters.register_parameters(self)
 
         # publish global obstacles
-        self.goal_zone_obstacles_pub = self.create_publisher(geo_msg.ShapeSet, '/planning/goal_zone_obstacles', 10)
+        self.goal_zone_obstacles_pub = self.create_publisher(
+            geo_msg.ShapeSet, '/planning/goal_zone_obstacles', 10)
 
         timer_period = 1/60 #seconds
         self.timer = self.create_timer(timer_period, self.gameplay_tick)
diff --git a/rj_gameplay/rj_gameplay/skill/intercept.py b/rj_gameplay/rj_gameplay/skill/intercept.py
index 3f2e8f5e973..2db6d385dfe 100644
--- a/rj_gameplay/rj_gameplay/skill/intercept.py
+++ b/rj_gameplay/rj_gameplay/skill/intercept.py
@@ -9,11 +9,12 @@
 import stp.skill as skill
 import stp.role as role
 import stp.action as action
-from rj_gameplay.action import intercept 
+from rj_gameplay.action import intercept
 from stp.skill.action_behavior import ActionBehavior
 import stp.rc as rc
 import numpy as np
 
+
 class IIntercept(skill.ISkill, ABC):
     ...
 
@@ -21,10 +22,13 @@ class IIntercept(skill.ISkill, ABC):
 """
 A skill version of intercept so that actions don't have to be called in tactics
 """
+
+
 # TODO: discuss collapsing skills/actions
 class Intercept(IIntercept):
-    
-    def __init__(self, robot: rc.Robot=None, target_point: np.ndarray=np.array([0.0, 0.0])):
+    def __init__(self,
+                 robot: rc.Robot = None,
+                 target_point: np.ndarray = np.array([0.0, 0.0])):
         self.robot = robot
         self.target_point = target_point
 
@@ -33,7 +37,8 @@ def __init__(self, robot: rc.Robot=None, target_point: np.ndarray=np.array([0.0,
             self.intercept = intercept.Intercept(self.robot.id, target_point)
         else:
             self.intercept = intercept.Intercept(None, target_point)
-        self.intercept_behavior = ActionBehavior('Intercept', self.intercept, self.robot)
+        self.intercept_behavior = ActionBehavior('Intercept', self.intercept,
+                                                 self.robot)
         self.root = self.intercept_behavior
         self.root.setup_with_descendants()
 
@@ -44,4 +49,4 @@ def tick(self, robot: rc.Robot, world_state: rc.WorldState):
         return self.root.tick_once(self.robot, world_state)
 
     def is_done(self, world_state) -> bool:
-    	return self.intercept.is_done(world_state)
+        return self.intercept.is_done(world_state)
diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
index dfa12985c8f..70ba95b76b2 100644
--- a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -11,7 +11,7 @@
 
 import rj_gameplay.eval
 import rj_gameplay.skill as skills
-from rj_gameplay.skill import move, receive #, intercept
+from rj_gameplay.skill import move, receive  #, intercept
 import stp.skill as skill
 import numpy as np
 # TODO: replace w/ global param server
@@ -23,6 +23,7 @@
 MIN_WALL_RAD = 0
 GOALIE_PCT_TO_BALL = 0.15
 
+
 class goalie_cost(role.CostFn):
     """Cost function for role request. Want only the designated goalie to be selected.
     """
@@ -37,10 +38,11 @@ def __call__(self, robot: rc.Robot, prev_result: Optional["RoleResult"],
             # this is a hacky way of doing goalie, until sub is fixed
             # TODO: fix goalie_id sub in gameplay node
             if robot.id == 0:
-            # if robot.id == world_state.game_info.goalie_id:
+                # if robot.id == world_state.game_info.goalie_id:
                 return -1.0
         return 999.0
 
+
 def get_goalie_pt(world_state: rc.WorldState) -> np.ndarray:
     """Finds point for goalie to best be in to block a shot.
     :return numpy point
@@ -51,16 +53,20 @@ def get_goalie_pt(world_state: rc.WorldState) -> np.ndarray:
 
     dir_vec = (ball_pt - goal_pt) / np.linalg.norm(ball_pt - goal_pt)
     # get in-between ball and goal, staying behind wall
-    dist_from_goal = min(GOALIE_PCT_TO_BALL * np.linalg.norm(ball_pt - goal_pt), MIN_WALL_RAD - RobotConstants.RADIUS * 2.1)
+    dist_from_goal = min(
+        GOALIE_PCT_TO_BALL * np.linalg.norm(ball_pt - goal_pt),
+        MIN_WALL_RAD - RobotConstants.RADIUS * 2.1)
     mid_pt = goal_pt + (dir_vec * dist_from_goal)
     return mid_pt
 
+
 # TODO: replace with intercept
 def get_block_pt(world_state: rc.WorldState, my_pos: np.ndarray) -> np.ndarray:
     pos = world_state.ball.pos
     vel = world_state.ball.vel
 
-    block_pt = np.array([(my_pos[1] - pos[1]) / vel[1] * vel[0] + pos[0], my_pos[1]])
+    block_pt = np.array([(my_pos[1] - pos[1]) / vel[1] * vel[0] + pos[0],
+                         my_pos[1]])
 
     return block_pt
 
@@ -93,35 +99,47 @@ def get_requests(self, world_state: rc.WorldState,
         :return: A list of role requests for move skills needed
         """
 
-        # TODO: this calculation is copy-pasted from wall_tactic 
+        # TODO: this calculation is copy-pasted from wall_tactic
         # put into common param file: https://www.geeksforgeeks.org/global-keyword-in-python/
 
         # dist is slightly greater than penalty box bounds
         box_w = world_state.field.penalty_long_dist_m
         box_h = world_state.field.penalty_short_dist_m
         line_w = world_state.field.line_width_m
-        MIN_WALL_RAD = RobotConstants.RADIUS + line_w + np.hypot(box_w / 2, box_h)
+        MIN_WALL_RAD = RobotConstants.RADIUS + line_w + np.hypot(
+            box_w / 2, box_h)
 
         role_requests = {}
         if world_state and world_state.ball.visible:
             ball_speed = np.linalg.norm(world_state.ball.vel)
-            ball_dist = np.linalg.norm(world_state.field.our_goal_loc - world_state.ball.pos)
+            ball_dist = np.linalg.norm(world_state.field.our_goal_loc -
+                                       world_state.ball.pos)
 
             if ball_speed < 1.0 and ball_dist < MIN_WALL_RAD - RobotConstants.RADIUS * 2.1:
                 # if ball is slow and inside goalie box, collect it
-                role_requests[self.receive_se] = [role.RoleRequest(role.Priority.HIGH, True, self.role_cost)]
+                role_requests[self.receive_se] = [
+                    role.RoleRequest(role.Priority.HIGH, True, self.role_cost)
+                ]
             else:
                 ball_to_goal_time = ball_dist / ball_speed
                 if ball_speed > 0 and ball_to_goal_time < 2:
                     # if ball is moving and coming at goal, move laterally to block ball
-                    self.move_se.skill.target_point = get_block_pt(world_state, get_goalie_pt(world_state))
+                    self.move_se.skill.target_point = get_block_pt(
+                        world_state, get_goalie_pt(world_state))
                     self.move_se.skill.face_point = world_state.ball.pos
-                    role_requests[self.move_se] = [role.RoleRequest(role.Priority.HIGH, True, self.role_cost)]
+                    role_requests[self.move_se] = [
+                        role.RoleRequest(role.Priority.HIGH, True,
+                                         self.role_cost)
+                    ]
                 else:
                     # else, track ball normally
-                    self.move_se.skill.target_point = get_goalie_pt(world_state)
+                    self.move_se.skill.target_point = get_goalie_pt(
+                        world_state)
                     self.move_se.skill.face_point = world_state.ball.pos
-                    role_requests[self.move_se] = [role.RoleRequest(role.Priority.HIGH, True, self.role_cost)]
+                    role_requests[self.move_se] = [
+                        role.RoleRequest(role.Priority.HIGH, True,
+                                         self.role_cost)
+                    ]
 
         return role_requests
 
diff --git a/rj_gameplay/rj_gameplay/tactic/wall_tactic.py b/rj_gameplay/rj_gameplay/tactic/wall_tactic.py
index 753db20dea2..518507c1d47 100644
--- a/rj_gameplay/rj_gameplay/tactic/wall_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/wall_tactic.py
@@ -20,6 +20,7 @@
 
 MIN_WALL_RAD = None
 
+
 class wall_cost(role.CostFn):
     """Cost function for role request.
     """
diff --git a/rj_gameplay/stp/rc.py b/rj_gameplay/stp/rc.py
index 740a8227cfd..e51e4b26091 100644
--- a/rj_gameplay/stp/rc.py
+++ b/rj_gameplay/stp/rc.py
@@ -425,7 +425,9 @@ def goal_height_m(self) -> float:
 class GameInfo:
     """State of the soccer game"""
 
-    __slots__ = ["__period", "__state", "__restart", "__our_restart", "__goalie_id"]
+    __slots__ = [
+        "__period", "__state", "__restart", "__our_restart", "__goalie_id"
+    ]
 
     __period: GamePeriod
     __state: GameState
@@ -434,7 +436,7 @@ class GameInfo:
     __goalie_id: int
 
     def __init__(self, period: GamePeriod, state: GameState,
-            restart: GameRestart, our_restart: bool, goalie_id: int):
+                 restart: GameRestart, our_restart: bool, goalie_id: int):
         self.__period = period
         self.__state = state
         self.__restart = restart
diff --git a/rj_gameplay/stp/utils/world_state_converter.py b/rj_gameplay/stp/utils/world_state_converter.py
index 8d8cefedae4..7d57029abb1 100644
--- a/rj_gameplay/stp/utils/world_state_converter.py
+++ b/rj_gameplay/stp/utils/world_state_converter.py
@@ -26,7 +26,7 @@ class RobotStatus():
     def __init__(self, robot_id: RobotId = None,
                  has_ball_sense: bool = None, kicker_charged: bool = None,
                  kicker_healthy: bool = None, lethal_fault: bool = None):
-        
+
         self.robot_id = robot_id
         self.has_ball_sense = has_ball_sense
         self.kicker_charged = kicker_charged
@@ -144,7 +144,8 @@ def gamestate_to_gameinfo(game_state_msg: msg.GameState) -> rc.GameInfo:
     restart = game_state_msg.restart
     our_restart = game_state_msg.our_restart
 
-    game_info = rc.GameInfo(period, state, restart, our_restart, None) # goalie_id set later
+    game_info = rc.GameInfo(period, state, restart, our_restart,
+                            None)  # goalie_id set later
 
     return game_info
 
@@ -251,4 +252,3 @@ def worldstate_creator(partial_world_state: PartialWorldState, robot_statuses: L
     world_state = rc.WorldState(our_robots, their_robots, partial_world_state.ball, game_info, field)
 
     return world_state
-    

From 9de733fffbab9d15bc0aa1510675357515439cbe Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Sat, 19 Jun 2021 12:15:21 -0400
Subject: [PATCH 21/28] goalie pub now on timer

---
 rj_gameplay/rj_gameplay/gameplay_node.py   | 11 ++---------
 soccer/src/soccer/referee/referee_base.cpp | 14 ++++++++++----
 soccer/src/soccer/referee/referee_base.hpp |  8 +++++++-
 3 files changed, 19 insertions(+), 14 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/gameplay_node.py b/rj_gameplay/rj_gameplay/gameplay_node.py
index df9aaabc90c..3ba73f1d75f 100644
--- a/rj_gameplay/rj_gameplay/gameplay_node.py
+++ b/rj_gameplay/rj_gameplay/gameplay_node.py
@@ -1,5 +1,6 @@
 import rclpy
 from rclpy.node import Node
+from rclpy.qos import QoSProfile, QoSDurabilityPolicy
 
 from rj_msgs import msg
 from rj_geometry_msgs import msg as geo_msg
@@ -39,6 +40,7 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
         self.world_state_sub = self.create_subscription(msg.WorldState, '/vision_filter/world_state', self.create_partial_world_state, 10)
         self.field_dimensions = self.create_subscription(msg.FieldDimensions, '/config/field_dimensions', self.create_field, 10)
         self.game_info = self.create_subscription(msg.GameState, '/referee/game_state', self.create_game_info, 10)
+
         self.goalie_id_sub = self.create_subscription(msg.Goalie,
                                                       '/referee/our_goalie',
                                                       self.create_goalie_id,
@@ -99,13 +101,6 @@ def create_game_info(self, msg: msg.GameState) -> None:
         """
         if msg is not None:
             self.game_info = conv.gamestate_to_gameinfo(msg)
-            # self.game_info.set_goalie_id(4)
-        """
-        if self.goalie_id is not None:
-            print("set game info")
-            print(self.goalie_id)
-            self.game_info.set_goalie_id(self.goalie_id)
-        """
 
     def create_field(self, msg: msg.FieldDimensions) -> None:
         """
@@ -118,8 +113,6 @@ def create_goalie_id(self, msg: msg.Goalie) -> None:
         """
         Set game_info's goalie_id based on goalie msg
         """
-        print("msg")
-        print(msg)
         if msg is not None and self.game_info is not None:
             self.goalie_id = msg.goalie_id
             self.game_info.set_goalie_id(msg.goalie_id)
diff --git a/soccer/src/soccer/referee/referee_base.cpp b/soccer/src/soccer/referee/referee_base.cpp
index 200161791a7..22183f64fc8 100644
--- a/soccer/src/soccer/referee/referee_base.cpp
+++ b/soccer/src/soccer/referee/referee_base.cpp
@@ -15,6 +15,7 @@ RefereeBase::RefereeBase(const std::string& name)
     their_team_info_pub_ =
         create_publisher<TeamInfoMsg>(referee::topics::kTheirInfoPub, keep_latest);
     game_state_pub_ = create_publisher<GameStateMsg>(referee::topics::kGameStatePub, keep_latest);
+
 }
 
 void RefereeBase::play() {
@@ -99,14 +100,19 @@ void RefereeBase::set_goalie(uint8_t goalie_id) {
 }
 
 void RefereeBase::send() {
+
     if (!has_any_info_) {
         return;
     }
 
     if (!goalie_valid_) {
-        GoalieMsg msg;
-        msg.goalie_id = blue_team_ ? blue_info_.goalie : yellow_info_.goalie;
-        goalie_id_pub_->publish(msg);
+        goalie_msg.goalie_id = blue_team_ ? blue_info_.goalie : yellow_info_.goalie;
+        goalie_id_pub_->publish(goalie_msg);
+        // publish the goalie_id on a timer, since transient_local QoS isn't working
+        // TODO (#1675): fix transient_local OR publish all similar msgs on timer
+        pub_timer_ = this->create_wall_timer(std::chrono::seconds(1), [this] () {
+                goalie_id_pub_->publish(goalie_msg);
+            });
         goalie_valid_ = true;
     }
 
@@ -145,4 +151,4 @@ void RefereeBase::update_team_color_from_names() {
     }
 }
 
-}  // namespace referee
\ No newline at end of file
+}  // namespace referee
diff --git a/soccer/src/soccer/referee/referee_base.hpp b/soccer/src/soccer/referee/referee_base.hpp
index 5ad4e63c6c4..1f71ed8f445 100644
--- a/soccer/src/soccer/referee/referee_base.hpp
+++ b/soccer/src/soccer/referee/referee_base.hpp
@@ -162,6 +162,12 @@ class RefereeBase : public rclcpp::Node {
     rclcpp::Publisher<TeamInfoMsg>::SharedPtr their_team_info_pub_;
     rclcpp::Publisher<GameStateMsg>::SharedPtr game_state_pub_;
 
+
+    // setup to publish goalie_msg on a timer, as it's only exposed on 
+    // gameplay start/halts and transient_local QoS is failing us
+    GoalieMsg goalie_msg;
+    rclcpp::TimerBase::SharedPtr pub_timer_;
+
     /**
      * @brief Update the team color from the names currently available in the
      * blue and yellow team information.
@@ -174,4 +180,4 @@ class RefereeBase : public rclcpp::Node {
     params::LocalROS2ParamProvider param_provider_;
 };
 
-}  // namespace referee
\ No newline at end of file
+}  // namespace referee

From 525ceb58b77cd118a7f912cfd70116119c597417 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Sat, 19 Jun 2021 12:20:42 -0400
Subject: [PATCH 22/28] update to reflect working goalie_id, rm debug prints,
 add comments

---
 .../rj_gameplay/tactic/goalie_tactic.py        | 18 +++++-------------
 1 file changed, 5 insertions(+), 13 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
index 70ba95b76b2..4d7fc28b4a6 100644
--- a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -24,21 +24,14 @@
 GOALIE_PCT_TO_BALL = 0.15
 
 
-class goalie_cost(role.CostFn):
+class GoalieCost(role.CostFn):
     """Cost function for role request. Want only the designated goalie to be selected.
     """
     def __call__(self, robot: rc.Robot, prev_result: Optional["RoleResult"],
                  world_state: rc.WorldState) -> float:
 
         if world_state.game_info is not None:
-            # TODO: clear debug prints
-            # print(world_state.game_info.our_restart)
-            # print(world_state.game_info.goalie_id)
-
-            # this is a hacky way of doing goalie, until sub is fixed
-            # TODO: fix goalie_id sub in gameplay node
-            if robot.id == 0:
-                # if robot.id == world_state.game_info.goalie_id:
+            if robot.id == world_state.game_info.goalie_id:
                 return -1.0
         return 999.0
 
@@ -60,7 +53,6 @@ def get_goalie_pt(world_state: rc.WorldState) -> np.ndarray:
     return mid_pt
 
 
-# TODO: replace with intercept
 def get_block_pt(world_state: rc.WorldState, my_pos: np.ndarray) -> np.ndarray:
     pos = world_state.ball.pos
     vel = world_state.ball.vel
@@ -77,11 +69,10 @@ def __init__(self):
         # create move SkillEntry
         self.move_se = tactic.SkillEntry(move.Move())
 
-        # TODO: replace w/ intercept
         self.receive_se = tactic.SkillEntry(receive.Receive())
 
         # TODO: rename cost_list to role_cost in other gameplay files
-        self.role_cost = goalie_cost()
+        self.role_cost = GoalieCost()
 
     def compute_props(self):
         pass
@@ -124,6 +115,7 @@ def get_requests(self, world_state: rc.WorldState,
                 ball_to_goal_time = ball_dist / ball_speed
                 if ball_speed > 0 and ball_to_goal_time < 2:
                     # if ball is moving and coming at goal, move laterally to block ball
+                    # TODO (#1676): replace this logic with a real intercept planner
                     self.move_se.skill.target_point = get_block_pt(
                         world_state, get_goalie_pt(world_state))
                     self.move_se.skill.face_point = world_state.ball.pos
@@ -156,7 +148,7 @@ def tick(self,
         if move_result and move_result[0].is_filled():
             skills.append(self.move_se)
         else:
-            # move first, then receive
+            # move skill takes priority
             receive_result = role_results[self.receive_se]
             if receive_result and receive_result[0].is_filled():
                 skills.append(self.receive_se)

From 178b0b51a71b97ef961dcb8fb5a286fd135b2e79 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Sat, 19 Jun 2021 12:20:56 -0400
Subject: [PATCH 23/28] make pretty-lines

---
 soccer/src/soccer/referee/referee_base.cpp | 7 ++-----
 soccer/src/soccer/referee/referee_base.hpp | 3 +--
 2 files changed, 3 insertions(+), 7 deletions(-)

diff --git a/soccer/src/soccer/referee/referee_base.cpp b/soccer/src/soccer/referee/referee_base.cpp
index 22183f64fc8..f6808f24a43 100644
--- a/soccer/src/soccer/referee/referee_base.cpp
+++ b/soccer/src/soccer/referee/referee_base.cpp
@@ -15,7 +15,6 @@ RefereeBase::RefereeBase(const std::string& name)
     their_team_info_pub_ =
         create_publisher<TeamInfoMsg>(referee::topics::kTheirInfoPub, keep_latest);
     game_state_pub_ = create_publisher<GameStateMsg>(referee::topics::kGameStatePub, keep_latest);
-
 }
 
 void RefereeBase::play() {
@@ -100,7 +99,6 @@ void RefereeBase::set_goalie(uint8_t goalie_id) {
 }
 
 void RefereeBase::send() {
-
     if (!has_any_info_) {
         return;
     }
@@ -110,9 +108,8 @@ void RefereeBase::send() {
         goalie_id_pub_->publish(goalie_msg);
         // publish the goalie_id on a timer, since transient_local QoS isn't working
         // TODO (#1675): fix transient_local OR publish all similar msgs on timer
-        pub_timer_ = this->create_wall_timer(std::chrono::seconds(1), [this] () {
-                goalie_id_pub_->publish(goalie_msg);
-            });
+        pub_timer_ = this->create_wall_timer(std::chrono::seconds(1),
+                                             [this]() { goalie_id_pub_->publish(goalie_msg); });
         goalie_valid_ = true;
     }
 
diff --git a/soccer/src/soccer/referee/referee_base.hpp b/soccer/src/soccer/referee/referee_base.hpp
index 1f71ed8f445..a905cfdb278 100644
--- a/soccer/src/soccer/referee/referee_base.hpp
+++ b/soccer/src/soccer/referee/referee_base.hpp
@@ -162,8 +162,7 @@ class RefereeBase : public rclcpp::Node {
     rclcpp::Publisher<TeamInfoMsg>::SharedPtr their_team_info_pub_;
     rclcpp::Publisher<GameStateMsg>::SharedPtr game_state_pub_;
 
-
-    // setup to publish goalie_msg on a timer, as it's only exposed on 
+    // setup to publish goalie_msg on a timer, as it's only exposed on
     // gameplay start/halts and transient_local QoS is failing us
     GoalieMsg goalie_msg;
     rclcpp::TimerBase::SharedPtr pub_timer_;

From a44c4c708108fdfc5e399661654eacf381c9299b Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Sat, 19 Jun 2021 12:22:47 -0400
Subject: [PATCH 24/28] fix comment in goalie

---
 rj_gameplay/rj_gameplay/tactic/goalie_tactic.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
index 4d7fc28b4a6..b33886ff7ef 100644
--- a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -1,4 +1,4 @@
-"""Tactic to build a wall between mark pt (e.g. ball) and defense pt (e.g. goal)."""
+"""Tactic to produce goalie behavior, which tracks the ball, moves to block if a shot on goal is taken, and stays within the goalie box (generally)."""
 
 from dataclasses import dataclass
 from typing import List, Optional

From 97548667f97e359b8ecaced32eb547a3e69c1646 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Sun, 20 Jun 2021 01:48:56 -0400
Subject: [PATCH 25/28] goalie now chips ball away when settled, made changes
 to some pivot kick related stp

---
 rj_gameplay/rj_gameplay/action/kick.py        |  2 +-
 rj_gameplay/rj_gameplay/play/basic_defense.py |  2 +-
 rj_gameplay/rj_gameplay/skill/pivot_kick.py   | 21 +++++++----
 .../rj_gameplay/tactic/goalie_tactic.py       | 37 +++++++++++++------
 4 files changed, 41 insertions(+), 21 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/action/kick.py b/rj_gameplay/rj_gameplay/action/kick.py
index 8ef8005fda4..24d4075e5e0 100644
--- a/rj_gameplay/rj_gameplay/action/kick.py
+++ b/rj_gameplay/rj_gameplay/action/kick.py
@@ -31,7 +31,7 @@ def tick(self, intent:RobotIntent) -> RobotIntent:
         new_intent = intent
         empty_command = EmptyMotionCommand()
         new_intent.motion_command.empty_command = [empty_command]
-        intent.kick_speed = self.kick_speed
+        new_intent.kick_speed = self.kick_speed
         new_intent.trigger_mode = 2
         new_intent.shoot_mode = self.chip
         new_intent.is_active = True
diff --git a/rj_gameplay/rj_gameplay/play/basic_defense.py b/rj_gameplay/rj_gameplay/play/basic_defense.py
index 8715a0bc39c..d94fdf0920e 100644
--- a/rj_gameplay/rj_gameplay/play/basic_defense.py
+++ b/rj_gameplay/rj_gameplay/play/basic_defense.py
@@ -14,7 +14,7 @@ class BasicDefense(play.IPlay):
     """
     def __init__(self):
         self.tactics = [
-            wall_tactic.WallTactic(3),
+            # wall_tactic.WallTactic(3),
             nmark_tactic.NMarkTactic(2),
             goalie_tactic.GoalieTactic()
         ]
diff --git a/rj_gameplay/rj_gameplay/skill/pivot_kick.py b/rj_gameplay/rj_gameplay/skill/pivot_kick.py
index 5c525a0010b..ec6602dbe2d 100644
--- a/rj_gameplay/rj_gameplay/skill/pivot_kick.py
+++ b/rj_gameplay/rj_gameplay/skill/pivot_kick.py
@@ -8,7 +8,9 @@
 
 import stp.skill as skill
 import stp.role as role
-import rj_gameplay.action as action
+# import rj_gameplay.action as action
+# TODO: figure out why the above import crashed sim
+from rj_gameplay.action import pivot, kick, capture
 from stp.skill.action_behavior import ActionBehavior, RobotActions
 from stp.skill.rj_sequence import RjSequence as Sequence
 import stp.rc as rc
@@ -32,13 +34,15 @@ def __init__(self, target_point: np.array, chip: bool, kick_speed: float, robot:
         self.kick_speed = kick_speed
         self.root = Sequence("Sequence")
         self.target_point = target_point
+
         if robot is not None:
-            self.pivot = action.pivot.Pivot(robot.id ,robot.pose[0:2], target_point, MAX_DRIBBLER_SPEED)
-            self.kick = action.kick.Kick(self.robot.id, self.chip, self.kick_speed)
+            self.pivot = pivot.Pivot(robot.id ,robot.pose[0:2], target_point, MAX_DRIBBLER_SPEED)
+            self.kick = kick.Kick(self.robot.id, self.chip, self.kick_speed)
         else:
-            self.pivot = action.pivot.Pivot(None, np.array([0.0,0.0]), target_point, MAX_DRIBBLER_SPEED)
-            self.kick = action.kick.Kick(self.robot, self.chip, self.kick_speed)
-        self.capture = action.capture.Capture()
+            self.pivot = pivot.Pivot(None, np.array([0.0,0.0]), target_point, MAX_DRIBBLER_SPEED)
+            self.kick = kick.Kick(None, self.chip, self.kick_speed)
+
+        self.capture = capture.Capture()
         self.capture_behavior = ActionBehavior('Capture', self.capture)
         self.pivot_behavior = ActionBehavior('Pivot', self.pivot) 
         self.kick_behavior = ActionBehavior('Kick', self.kick)
@@ -47,10 +51,13 @@ def __init__(self, target_point: np.array, chip: bool, kick_speed: float, robot:
 
     def tick(self, robot: rc.Robot, world_state: rc.WorldState) -> RobotActions:
         self.robot = robot
+        self.pivot.robot_id = robot.id
+        self.kick.robot_id = robot.id
+
         self.pivot.pivot_point = world_state.ball.pos
         self.pivot.target_point = self.target_point
         actions = self.root.tick_once(robot, world_state)
         return actions
 
     def is_done(self, world_state: rc.WorldState) -> bool:
-        return self.pivot.is_done(world_state) and self.kick.is_done(world_state)
\ No newline at end of file
+        return self.kick.is_done(world_state)
diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
index b33886ff7ef..98407c3af34 100644
--- a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -11,7 +11,7 @@
 
 import rj_gameplay.eval
 import rj_gameplay.skill as skills
-from rj_gameplay.skill import move, receive  #, intercept
+from rj_gameplay.skill import move, receive, pivot_kick  #, intercept
 import stp.skill as skill
 import numpy as np
 # TODO: replace w/ global param server
@@ -66,10 +66,10 @@ def get_block_pt(world_state: rc.WorldState, my_pos: np.ndarray) -> np.ndarray:
 class GoalieTactic(tactic.ITactic):
     def __init__(self):
 
-        # create move SkillEntry
+        # init skills
         self.move_se = tactic.SkillEntry(move.Move())
-
         self.receive_se = tactic.SkillEntry(receive.Receive())
+        self.pivot_kick_se = tactic.SkillEntry(pivot_kick.PivotKick(target_point = np.array([0.0, 6.0]), chip=True, kick_speed=4.0))
 
         # TODO: rename cost_list to role_cost in other gameplay files
         self.role_cost = GoalieCost()
@@ -107,10 +107,18 @@ def get_requests(self, world_state: rc.WorldState,
                                        world_state.ball.pos)
 
             if ball_speed < 1.0 and ball_dist < MIN_WALL_RAD - RobotConstants.RADIUS * 2.1:
-                # if ball is slow and inside goalie box, collect it
-                role_requests[self.receive_se] = [
-                    role.RoleRequest(role.Priority.HIGH, True, self.role_cost)
-                ]
+                self.move_se = tactic.SkillEntry(move.Move())
+                if not self.receive_se.skill.is_done(world_state): 
+                    # if ball is slow and inside goalie box, collect it
+                    role_requests[self.receive_se] = [
+                        role.RoleRequest(role.Priority.HIGH, True, self.role_cost)
+                    ]
+                else:
+                    # if ball has been stopped already, chip toward center field
+                    self.pivot_kick_se.skill.target_point = np.array([0.0, 6.0])
+                    role_requests[self.pivot_kick_se] = [
+                        role.RoleRequest(role.Priority.HIGH, True, self.role_cost)
+                    ]
             else:
                 ball_to_goal_time = ball_dist / ball_speed
                 if ball_speed > 0 and ball_to_goal_time < 2:
@@ -145,13 +153,18 @@ def tick(self,
         skills = []
 
         move_result = role_results[self.move_se]
+        receive_result = role_results[self.receive_se]
+        pivot_kick_result = role_results[self.pivot_kick_se]
+
+        # move skill takes priority
         if move_result and move_result[0].is_filled():
             skills.append(self.move_se)
-        else:
-            # move skill takes priority
-            receive_result = role_results[self.receive_se]
-            if receive_result and receive_result[0].is_filled():
-                skills.append(self.receive_se)
+        elif receive_result and receive_result[0].is_filled():
+            skills.append(self.receive_se)
+        elif pivot_kick_result and pivot_kick_result[0].is_filled():
+            skills.append(self.pivot_kick_se)
+            print("--")
+            print(skills[0].skill.target_point)
 
         return skills
 

From 5f277326b6be1352f1e0056339684b7788068dae Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Sun, 20 Jun 2021 02:03:31 -0400
Subject: [PATCH 26/28] up chip speed, enable wall

---
 rj_gameplay/rj_gameplay/play/basic_defense.py   | 2 +-
 rj_gameplay/rj_gameplay/tactic/goalie_tactic.py | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/play/basic_defense.py b/rj_gameplay/rj_gameplay/play/basic_defense.py
index d94fdf0920e..8715a0bc39c 100644
--- a/rj_gameplay/rj_gameplay/play/basic_defense.py
+++ b/rj_gameplay/rj_gameplay/play/basic_defense.py
@@ -14,7 +14,7 @@ class BasicDefense(play.IPlay):
     """
     def __init__(self):
         self.tactics = [
-            # wall_tactic.WallTactic(3),
+            wall_tactic.WallTactic(3),
             nmark_tactic.NMarkTactic(2),
             goalie_tactic.GoalieTactic()
         ]
diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
index 98407c3af34..a04a1da0377 100644
--- a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -69,7 +69,7 @@ def __init__(self):
         # init skills
         self.move_se = tactic.SkillEntry(move.Move())
         self.receive_se = tactic.SkillEntry(receive.Receive())
-        self.pivot_kick_se = tactic.SkillEntry(pivot_kick.PivotKick(target_point = np.array([0.0, 6.0]), chip=True, kick_speed=4.0))
+        self.pivot_kick_se = tactic.SkillEntry(pivot_kick.PivotKick(target_point = np.array([0.0, 6.0]), chip=True, kick_speed=6.0))
 
         # TODO: rename cost_list to role_cost in other gameplay files
         self.role_cost = GoalieCost()

From 1d409295ab2e347ef402583fb17c94b349e447ff Mon Sep 17 00:00:00 2001
From: vagrant <hgynai732@gmail.com>
Date: Sun, 20 Jun 2021 21:01:51 +0000
Subject: [PATCH 27/28] allowed for repeated pivot kicking

---
 rj_gameplay/rj_gameplay/action/kick.py          | 4 ++--
 rj_gameplay/rj_gameplay/action/pivot.py         | 1 +
 rj_gameplay/rj_gameplay/skill/pivot_kick.py     | 5 ++++-
 rj_gameplay/rj_gameplay/skill/receive.py        | 2 ++
 rj_gameplay/rj_gameplay/tactic/goalie_tactic.py | 6 +++---
 5 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/rj_gameplay/rj_gameplay/action/kick.py b/rj_gameplay/rj_gameplay/action/kick.py
index 24d4075e5e0..fa60ab590ab 100644
--- a/rj_gameplay/rj_gameplay/action/kick.py
+++ b/rj_gameplay/rj_gameplay/action/kick.py
@@ -10,8 +10,8 @@
 from rj_msgs.msg import RobotIntent, EmptyMotionCommand
 from rj_geometry_msgs.msg import Point
 
-KICK_DOT_THRESHOLD = 0.5
-KICK_BALL_SPEED_THRESHOLD = 1.0
+KICK_DOT_THRESHOLD = 0.4
+KICK_BALL_SPEED_THRESHOLD = 0.9
 
 class IKick(action.IAction, ABC):
     def done(self) -> bool:
diff --git a/rj_gameplay/rj_gameplay/action/pivot.py b/rj_gameplay/rj_gameplay/action/pivot.py
index c65e4378c2d..de5e569b9ec 100644
--- a/rj_gameplay/rj_gameplay/action/pivot.py
+++ b/rj_gameplay/rj_gameplay/action/pivot.py
@@ -25,6 +25,7 @@ def tick(self, intent: RobotIntent) -> None:
         pivot_command.pivot_point = Point(x=self.pivot_point[0], y=self.pivot_point[1])
         pivot_command.pivot_target = Point(x=self.target_point[0], y=self.target_point[1])
         new_intent.motion_command.pivot_command = [pivot_command]
+        new_intent.trigger_mode = new_intent.TRIGGER_MODE_STAND_DOWN
         new_intent.dribbler_speed = self.dribble_speed
         new_intent.is_active = True
         return new_intent
diff --git a/rj_gameplay/rj_gameplay/skill/pivot_kick.py b/rj_gameplay/rj_gameplay/skill/pivot_kick.py
index ec6602dbe2d..fb798b05077 100644
--- a/rj_gameplay/rj_gameplay/skill/pivot_kick.py
+++ b/rj_gameplay/rj_gameplay/skill/pivot_kick.py
@@ -57,7 +57,10 @@ def tick(self, robot: rc.Robot, world_state: rc.WorldState) -> RobotActions:
         self.pivot.pivot_point = world_state.ball.pos
         self.pivot.target_point = self.target_point
         actions = self.root.tick_once(robot, world_state)
+        self.pivot.robot_id = self.robot.id
+        self.kick.robot_id = self.robot.id
+        self.capture.robot_id = self.robot.id
         return actions
 
     def is_done(self, world_state: rc.WorldState) -> bool:
-        return self.kick.is_done(world_state)
+        return self.pivot.is_done(world_state) and self.kick.is_done(world_state)
diff --git a/rj_gameplay/rj_gameplay/skill/receive.py b/rj_gameplay/rj_gameplay/skill/receive.py
index 71deabf0797..6c39767309d 100644
--- a/rj_gameplay/rj_gameplay/skill/receive.py
+++ b/rj_gameplay/rj_gameplay/skill/receive.py
@@ -46,6 +46,8 @@ def __init__(self,
     def tick(self, robot:rc.Robot, world_state:rc.WorldState): #returns dict of robot and actions
         self.robot = robot
         actions = self.root.tick_once(self.robot, world_state)
+        self.capture.robot_id = self.robot.id
+        self.receive.robot_id = self.robot.id
         return actions
 
     def is_done(self, world_state:rc.WorldState):
diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
index 98407c3af34..134a8c3b47e 100644
--- a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -69,7 +69,7 @@ def __init__(self):
         # init skills
         self.move_se = tactic.SkillEntry(move.Move())
         self.receive_se = tactic.SkillEntry(receive.Receive())
-        self.pivot_kick_se = tactic.SkillEntry(pivot_kick.PivotKick(target_point = np.array([0.0, 6.0]), chip=True, kick_speed=4.0))
+        self.pivot_kick_se = tactic.SkillEntry(pivot_kick.PivotKick(target_point = np.array([0.0, 6.0]), chip=False, kick_speed=4.0))
 
         # TODO: rename cost_list to role_cost in other gameplay files
         self.role_cost = GoalieCost()
@@ -140,6 +140,8 @@ def get_requests(self, world_state: rc.WorldState,
                         role.RoleRequest(role.Priority.HIGH, True,
                                          self.role_cost)
                     ]
+        if self.pivot_kick_se.skill.is_done(world_state):
+            self.pivot_kick_se = tactic.SkillEntry(pivot_kick.PivotKick(target_point = np.array([0.0, 6.0]), chip=False, kick_speed=4.0))
 
         return role_requests
 
@@ -163,8 +165,6 @@ def tick(self,
             skills.append(self.receive_se)
         elif pivot_kick_result and pivot_kick_result[0].is_filled():
             skills.append(self.pivot_kick_se)
-            print("--")
-            print(skills[0].skill.target_point)
 
         return skills
 

From 282f503b92a52aae03d53b2764a2cb4568e5d683 Mon Sep 17 00:00:00 2001
From: Kevin Fu <kevin.fu842@gmail.com>
Date: Sun, 20 Jun 2021 17:08:25 -0400
Subject: [PATCH 28/28] chip on re-pivot

---
 rj_gameplay/rj_gameplay/tactic/goalie_tactic.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
index f78df54e53f..62265735c02 100644
--- a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
+++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py
@@ -141,7 +141,7 @@ def get_requests(self, world_state: rc.WorldState,
                                          self.role_cost)
                     ]
         if self.pivot_kick_se.skill.is_done(world_state):
-            self.pivot_kick_se = tactic.SkillEntry(pivot_kick.PivotKick(None, target_point = np.array([0.0, 6.0]), chip=False, kick_speed=4.0))
+            self.pivot_kick_se = tactic.SkillEntry(pivot_kick.PivotKick(None, target_point = np.array([0.0, 6.0]), chip=True, kick_speed=4.0))
 
         return role_requests