Merge pull request #1674 from RoboJackets/new_passing_final_adjustments

Passing final comments

rj_gameplay/rj_gameplay/action/kick.py

@@ -10,6 +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
 
 class IKick(action.IAction, ABC):
     def done(self) -> bool:
@@ -43,6 +45,6 @@ def is_done(self, world_state:rc.WorldState) -> bool:
         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:
+        if dot_product > KICK_DOT_THRESHOLD and np.linalg.norm(world_state.ball.vel) > KICK_BALL_SPEED_THRESHOLD:
             return True
         return False

rj_gameplay/rj_gameplay/action/pivot.py

@@ -31,9 +31,9 @@ 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
+        angle_threshold = np.pi / 180 #One dgree
         #TODO: Make these local params
-        stopped_threshold = 1*10**(-5)
+        stopped_threshold = 0.5 * (np.pi / 180) #0.5 degrees a second
         if self.robot_id is None:
             return False
         robot = world_state.our_robots[self.robot_id]

rj_gameplay/rj_gameplay/action/receive.py

@@ -8,6 +8,8 @@
 import numpy as np
 from rj_msgs.msg import RobotIntent, SettleMotionCommand
 
+SETTLE_BALL_SPEED_THRESHOLD = 1.0
+
 class Receive(action.IAction):
     """
     Receive action
@@ -29,6 +31,6 @@ def is_done(self, world_state) -> bool:
         if self.robot_id is None:
             return False
         #TODO: Use local params for this threshold
-        if world_state.our_robots[self.robot_id].has_ball_sense or np.linalg.norm(world_state.ball.vel) < 10**(-6):
+        if world_state.our_robots[self.robot_id].has_ball_sense or np.linalg.norm(world_state.ball.vel) < SETTLE_BALL_SPEED_THRESHOLD:
             return True
         return False

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, test_pass
+from rj_gameplay.play import line_up, passing_tactic_play
 from typing import List, Optional, Tuple
 
 NUM_ROBOTS = 16
@@ -25,7 +25,7 @@ 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, test_pass.PassPlay())
+        return (None, passing_tactic_play.PassPlay())
 
 class GameplayNode(Node):
     """

rj_gameplay/rj_gameplay/play/test_pass.py → rj_gameplay/rj_gameplay/play/passing_tactic_play.py

@@ -32,8 +32,6 @@ def tick(
         role_requests: play.RoleRequests = {}
         if not self.pass_tactic.is_done(world_state):
             role_requests[self.pass_tactic] = self.pass_tactic.get_requests(world_state, None)
-        else:
-            pass
         # 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)

rj_gameplay/rj_gameplay/skill/receive.py

@@ -34,7 +34,7 @@ def __init__(self,
             self.receive = receive.Receive(self.robot.id)
             self.capture = capture.Capture(self.robot.id)
         else:
-            self.receive = receive.Receive(self.robot)
+            self.receive = receive.Receive()
             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 ReceiverCost(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 PasserCost(role.CostFn):
     """
     A cost function for how to choose a robot that will pass
     TODO: Implement a better cost function
@@ -61,8 +61,8 @@ def __init__(self, target_point:np.ndarray):
         self.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 = ReceiverCost(target_point)
+        self.Passer_cost = PasserCost()
 
     def compute_props(self):
         pass
@@ -98,7 +98,7 @@ def tick(self, role_results:tactic.RoleResults, world_state:rc.WorldState) -> Li
 
         if pivot_result and receive_result and pivot_result[0].is_filled() and receive_result[0].is_filled():
             self.pivot_kick.skill.target_point = np.array(receive_result[0].role.robot.pose[0:2])
-            if self.pivot_kick.skill.pivot.is_done(world_state):
+            if self.pivot_kick.skill.kick.is_done(world_state):
                 return [self.pivot_kick, self.receive]
             else:
                 return [self.pivot_kick]