Revert "New gameplay passing"

rj_gameplay/rj_gameplay/action/capture.py

@@ -11,6 +11,7 @@
 class Capture(action.IAction):
     """
     Capture action
+    TODO: update with actions implementation
     """
 
     def __init__(self, robot_id: int = None):
@@ -21,10 +22,11 @@ def tick(self, intent) -> None:
         collect_command = CollectMotionCommand()
         intent.motion_command.collect_command = [collect_command] 
         intent.dribbler_speed = 1.0
-        intent.is_active = True
+        intent.active = True
+
         return intent
 
     def is_done(self, world_state) -> bool:
-        if not self.robot_id is None and world_state.our_robots[self.robot_id].has_ball_sense:
+        if world_state.our_robots[self.robot_id].has_ball_sense:
             return True
         return False

rj_gameplay/rj_gameplay/action/kick.py

@@ -6,9 +6,6 @@
 import stp.action as action
 import numpy as np
 import stp.rc as rc
-from typing import Optional
-from rj_msgs.msg import RobotIntent, EmptyMotionCommand
-from rj_geometry_msgs.msg import Point
 
 
 class IKick(action.IAction, ABC):
@@ -19,30 +16,19 @@ def done(self) -> bool:
 class Kick(IKick):
     """
     Kick action
+    TODO: update with actions implementation
     """
-    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
-
-    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.trigger_mode = 2
-        new_intent.shoot_mode = self.chip
-        new_intent.is_active = True
-        return new_intent
-
-    def is_done(self, world_state:rc.WorldState) -> bool:
-        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
+    def __init__(self, point: np.ndarray):
+        self.point = point
+        self.count = -1
+        #for stub
+
+    def tick(self, robot: rc.Robot, ctx: action.Ctx) -> None:
+        print('robot:', robot.id, 'kicking')
+        self.count += 1
+
+    def done(self) -> bool:
+        return self.count == 1
+
+    def fail(self):
         return False

rj_gameplay/rj_gameplay/action/pivot.py

@@ -13,37 +13,20 @@
 
 class Pivot(action.IFiniteAction):
 
-    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
+    """
+    Pivot Skill
+    """
+    def __init__(self, robot_id : int, pivot_point: np.ndarray, target_point: np.ndarray, priority : int = 1):
         self.pivot_point = pivot_point
         self.target_point = target_point
-        self.dribble_speed = dribble_speed
 
-    def tick(self, intent: RobotIntent) -> None:
-        new_intent = intent
+    def tick(self, intent) -> None:
         pivot_command = PivotMotionCommand()
         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.dribbler_speed = self.dribble_speed
-        new_intent.is_active = True
-        return new_intent
-
-    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
-        robot = world_state.our_robots[self.robot_id]
-        robot_pos_to_target = self.target_point - robot.pose[0:2]
-        robot_to_target_unit =  robot_pos_to_target / np.linalg.norm(robot_pos_to_target)
-        heading_vect = np.array([np.cos(robot.pose[2]), np.sin(robot.pose[2])])
-        dot_product = np.dot(heading_vect, robot_to_target_unit)
-        angle = np.arccos(dot_product)
-        if (angle < angle_threshold) and (abs(world_state.our_robots[self.robot_id].twist[2]) < stopped_threshold):
-            return True
-        else:
-            return False
+        intent.motion_command.pivot_motion_command = [pivot_command]
+        intent.active = True
 
+    def is_done(self, world_state) -> bool:
+        #vec = self.target_point
+        #world_state.our_robots[self.robot_id].pose[2]
+        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 defensive_clear 
 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, line_up.LineUp())
 
 class GameplayNode(Node):
     """

rj_gameplay/rj_gameplay/skill/capture.py

@@ -12,7 +12,6 @@
 from rj_gameplay.action import capture
 from stp.skill.action_behavior import ActionBehavior
 import stp.rc as rc
-from typing import Optional
 
 class ICapture(skill.ISkill, ABC):
     ...
@@ -23,8 +22,8 @@ class ICapture(skill.ISkill, ABC):
 """
 class Capture(ICapture):
 
-    def __init__(self, robot: Optional[rc.Robot]=None):
-        self.robot = robot
+    def __init__(self):
+        self.robot: rc.Robot = None
         self.__name__ = 'Capture'
         self.capture = capture.Capture()
         self.capture_behavior = ActionBehavior('Capture', self.capture, self.robot)

rj_gameplay/rj_gameplay/skill/pivot_kick.py

@@ -9,13 +9,10 @@
 import stp.skill as skill
 import stp.role as role
 import rj_gameplay.action as action
-from stp.skill.action_behavior import ActionBehavior, RobotActions
-from stp.skill.rj_sequence import RjSequence as Sequence
+from stp.skill.action_behavior import ActionBehavior
 import stp.rc as rc
 import numpy as np
 
-MAX_DRIBBLER_SPEED = 1.0
-
 class IPivotKick(skill.ISkill, ABC):
     ...
 
@@ -24,33 +21,18 @@ class PivotKick(IPivotKick):
     A pivot kick skill
     """
 
-    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 target 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, 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, MAX_DRIBBLER_SPEED)
-            self.kick = action.kick.Kick(self.robot, self.chip, self.kick_speed)
+    def __init__(self, role: role.Role, target_point: np.array) -> None:
+        self.robot = role.robot
+        self.root = py_trees.composites.Sequence("Sequence")
         self.capture = action.capture.Capture()
+        self.pivot = action.pivot.Pivot(self.robot.pos, self.robot.pose, target_point)
+        self.kick = action.kick.Kick(target_point)
         self.capture_behavior = ActionBehavior('Capture', self.capture)
         self.pivot_behavior = ActionBehavior('Pivot', self.pivot) 
         self.kick_behavior = ActionBehavior('Kick', self.kick)
         self.root.add_children([self.capture_behavior, self.pivot_behavior, self.kick_behavior])
         self.root.setup_with_descendants()
 
-    def tick(self, robot: rc.Robot, world_state: rc.WorldState) -> RobotActions:
-        self.robot = robot
-        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)
+    def tick(self, world_state: rc.WorldState, robot: rc.Robot) -> None:
+        self.root.tick_once(robot)
+        # TODO: change so this properly returns the actions intent messages