New gameplay passing

rj_gameplay/rj_gameplay/action/capture.py

@@ -22,7 +22,6 @@ def tick(self, intent) -> None:
         intent.motion_command.collect_command = [collect_command] 
         intent.dribbler_speed = 1.0
         intent.is_active = True
-
         return intent
 
     def is_done(self, world_state) -> bool:

rj_gameplay/rj_gameplay/action/kick.py

@@ -20,8 +20,7 @@ class Kick(IKick):
     """
     Kick action
     """
-    def __init__(self, robot_id:Optional[int]=None, chip:Optional[bool]=False, kick_speed:Optional[float]=255.0) -> None:
-        #TODO: Cahnge kick speed to use max_kick_speed param for default value
+    def __init__(self, robot_id:int, chip:bool, kick_speed:float) -> None:
         self.robot_id = robot_id
         self.chip = chip
         self.kick_speed = kick_speed
@@ -37,4 +36,13 @@ def tick(self, intent:RobotIntent) -> RobotIntent:
         return new_intent
 
     def is_done(self, world_state:rc.WorldState) -> bool:
-        return np.linalg.norm(world_state.ball.vel) > 1
+        if self.robot_id is None:
+            return False
+        ball_vel_unit = world_state.ball.vel / np.linalg.norm(world_state.ball.vel)
+        heading_angle = world_state.our_robots[self.robot_id].pose[2]
+        heading_vect = np.array([np.cos(heading_angle), np.sin(heading_angle)])
+        dot_product = np.dot(heading_vect, ball_vel_unit)
+        #TODO: Make this threshold a local param
+        if dot_product > 0.1:
+            return True
+        return False

rj_gameplay/rj_gameplay/action/pivot.py

@@ -13,7 +13,7 @@
 
 class Pivot(action.IFiniteAction):
 
-    def __init__(self, robot_id:int, pivot_point:np.ndarray, target_point:np.ndarray, dribble_speed:float=1.0, priority:int=1):
+    def __init__(self, robot_id:int, pivot_point:np.ndarray, target_point:np.ndarray, dribble_speed:float, priority:int=1):
         self.robot_id = robot_id
         self.pivot_point = pivot_point
         self.target_point = target_point
@@ -32,6 +32,7 @@ def tick(self, intent: RobotIntent) -> None:
     def is_done(self, world_state:rc.WorldState) -> bool:
         #TODO: Change this when we get action state feedback
         angle_threshold = 0.1
+        #TODO: Make these local params
         stopped_threshold = 1*10**(-5)
         if self.robot_id is None:
             return False

rj_gameplay/rj_gameplay/action/receive.py

@@ -13,22 +13,20 @@ class Receive(action.IAction):
     Receive action
     """
 
-    def __init__(self, robot_id: int = None, one_touch_target:np.ndarray = None):
+    def __init__(self, robot_id: int = None):
         self.robot_id = robot_id
-        self.one_touch_target = one_touch_target
 
 
     def tick(self, intent) -> None:
         settle_command = SettleMotionCommand()
-        if self.one_touch_target is not None:
-            settle_command.maybe_target = self.one_touch_target
         intent.motion_command.settle_command = [settle_command]
         intent.dribbler_speed = 1.0
         intent.is_active = True
 
         return intent
 
     def is_done(self, world_state) -> bool:
-        if np.linalg.norm(world_state.ball.vel) < 0.1:
+        #TODO: Use local params for this threshold
+        if np.linalg.norm(world_state.ball.vel) < 0.005:
             return True
         return False

rj_gameplay/rj_gameplay/skill/pivot_kick.py

@@ -14,6 +14,8 @@
 import stp.rc as rc
 import numpy as np
 
+MAX_DRIBBLER_SPEED = 1.0
+
 class IPivotKick(skill.ISkill, ABC):
     ...
 
@@ -22,17 +24,21 @@ class PivotKick(IPivotKick):
     A pivot kick skill
     """
 
-    def __init__(self, target_point:np.array, robot:rc.Robot=None) -> None:
+    def __init__(self, target_point:np.array, chip:bool, kick_speed:float, robot:rc.Robot=None) -> None:
+        #TODO: Have something which automatically determines kick speed based on targert point distance
         self.__name__ = 'pivot kick'
         self.robot = robot
+        self.chip= chip
+        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)
+            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)
         else:
-            self.pivot = action.pivot.Pivot(None, np.array([0.0,0.0]), target_point)
+            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.kick = action.kick.Kick()
         self.capture_behavior = ActionBehavior('Capture', self.capture)
         self.pivot_behavior = ActionBehavior('Pivot', self.pivot) 
         self.kick_behavior = ActionBehavior('Kick', self.kick)

rj_gameplay/rj_gameplay/skill/receive.py

@@ -27,16 +27,14 @@ class IReceive(skill.ISkill, ABC):
 class Receive(IReceive):
 
     def __init__(self,
-            robot:rc.Robot = None,
-            one_touch_target:np.ndarray = None):
+            robot:rc.Robot = None):
 
         self.robot = robot
-        self.one_touch_target = one_touch_target
         if self.robot is not None:
-            self.receive = receive.Receive(self.robot.id, self.one_touch_target)
+            self.receive = receive.Receive(self.robot.id)
             self.capture = capture.Capture(self.robot.id)
         else:
-            self.receive = receive.Receive(self.robot, self.one_touch_target)
+            self.receive = receive.Receive(self.robot)
             self.capture = capture.Capture()
         self.receive_behavior = ActionBehavior('Receive', self.receive)
         self.capture_behavior = ActionBehavior('Capture', self.capture)

rj_gameplay/rj_gameplay/tactic/pass_tactic.py

@@ -14,7 +14,7 @@
 import numpy as np
 
 
-class receiver_cost(role.CostFn):
+class Receiver_cost(role.CostFn):
     """
     A cost function for how to choose a robot to pass to
     TODO: Implement a better cost function
@@ -33,7 +33,7 @@ def __call__(
             return 0.0
         return 1.0
 
-class passer_cost(role.CostFn):
+class Passer_cost(role.CostFn):
     """
     A cost function for how to choose a robot that will pass
     TODO: Implement a better cost function
@@ -59,10 +59,10 @@ class Pass(tactic.ITactic):
 
     def __init__(self, target_point:np.ndarray):
         self.target_point = target_point
-        self.pivot_kick = tactic.SkillEntry(pivot_kick.PivotKick(target_point = target_point))
+        self.pivot_kick = tactic.SkillEntry(pivot_kick.PivotKick(target_point = target_point, chip=False, kick_speed=4.0))
         self.receive = tactic.SkillEntry(receive.Receive())
-        self.receiver_cost = receiver_cost(target_point)
-        self.passer_cost = passer_cost()
+        self.receiver_cost = Receiver_cost(target_point)
+        self.Passer_cost = Passer_cost()
 
     def compute_props(self):
         pass
@@ -81,7 +81,7 @@ def get_requests(
 
         role_requests: tactic.RoleRequests = {}
 
-        passer_request = role.RoleRequest(role.Priority.HIGH, True, self.passer_cost)
+        passer_request = role.RoleRequest(role.Priority.HIGH, True, self.Passer_cost)
         role_requests[self.pivot_kick] = [passer_request]
         receive_request = role.RoleRequest(role.Priority.HIGH, True, self.receiver_cost)
         role_requests[self.receive] = [receive_request]

rj_gameplay/stp/coordinator.py

@@ -6,7 +6,6 @@
 import stp.role.assignment as assignment
 import stp.situation
 from rj_msgs import msg
-from rj_geometry_msgs.msg import Point
 
 NUM_ROBOTS = 16