Merge pull request #1644 from RoboJackets/new_gameplay_passing

New gameplay passing

rj_gameplay/rj_gameplay/action/capture.py

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

rj_gameplay/rj_gameplay/action/kick.py

@@ -6,6 +6,9 @@
 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):
@@ -16,19 +19,30 @@ def done(self) -> bool:
 class Kick(IKick):
     """
     Kick action
-    TODO: update with actions implementation
     """
-    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):
+    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
         return False

rj_gameplay/rj_gameplay/action/pivot.py

@@ -13,20 +13,37 @@
 
 class Pivot(action.IFiniteAction):
 
-    """
-    Pivot Skill
-    """
-    def __init__(self, robot_id : int, pivot_point: np.ndarray, target_point: np.ndarray, 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
+        self.dribble_speed = dribble_speed
 
-    def tick(self, intent) -> None:
+    def tick(self, intent: RobotIntent) -> None:
+        new_intent = intent
         pivot_command = PivotMotionCommand()
         pivot_command.pivot_point = Point(x=self.pivot_point[0], y=self.pivot_point[1])
-        intent.motion_command.pivot_motion_command = [pivot_command]
-        intent.active = True
+        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
 
-    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/action/receive.py

@@ -0,0 +1,34 @@
+"""This module contains the interface and action for receive."""
+
+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, SettleMotionCommand
+
+class Receive(action.IAction):
+    """
+    Receive action
+    """
+
+    def __init__(self, robot_id: int = None):
+        self.robot_id = robot_id
+
+
+    def tick(self, intent) -> None:
+        settle_command = SettleMotionCommand()
+        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 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):
+            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 defensive_clear 
+from rj_gameplay.play import line_up, test_pass
 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, line_up.LineUp())
+        return (None, test_pass.PassPlay())
 
 class GameplayNode(Node):
     """

rj_gameplay/rj_gameplay/play/test_pass.py

@@ -0,0 +1,53 @@
+import stp.play as play
+import stp.tactic as tactic
+
+from rj_gameplay.tactic import pass_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
+import numpy as np
+
+class PassPlay(play.IPlay):
+    """A play which makes one robot pass to another
+    """
+
+    def __init__(self):
+        self.target_point = np.array([1.0,1.0])
+        self.pass_tactic = pass_tactic.Pass(self.target_point)
+        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 = {}
+        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)
+        role_results = play.unflatten_results(flat_results)
+
+        # Get list of all skills with assigned roles from tactics
+        skill_dict = {}
+        if not self.pass_tactic.is_done(world_state):
+            skills = self.pass_tactic.tick(role_results[self.pass_tactic], world_state)
+            skill_dict.update(role_results[self.pass_tactic])
+        else:
+            skills = []
+
+        return (skill_dict, skills)
+
+    def is_done(self, world_state: rc.WorldState):
+        return self.pass_tactic.is_done(world_state)

rj_gameplay/rj_gameplay/skill/capture.py

@@ -12,6 +12,7 @@
 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):
     ...
@@ -22,8 +23,8 @@ class ICapture(skill.ISkill, ABC):
 """
 class Capture(ICapture):
 
-    def __init__(self):
-        self.robot: rc.Robot = None
+    def __init__(self, robot: Optional[rc.Robot]=None):
+        self.robot = robot
         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,10 +9,13 @@
 import stp.skill as skill
 import stp.role as role
 import rj_gameplay.action as action
-from stp.skill.action_behavior import ActionBehavior
+from stp.skill.action_behavior import ActionBehavior, RobotActions
+from stp.skill.rj_sequence import RjSequence as Sequence
 import stp.rc as rc
 import numpy as np
 
+MAX_DRIBBLER_SPEED = 1.0
+
 class IPivotKick(skill.ISkill, ABC):
     ...
 
@@ -21,18 +24,33 @@ class PivotKick(IPivotKick):
     A pivot kick skill
     """
 
-    def __init__(self, role: role.Role, target_point: np.array) -> None:
-        self.robot = role.robot
-        self.root = py_trees.composites.Sequence("Sequence")
+    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)
         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, world_state: rc.WorldState, robot: rc.Robot) -> None:
-        self.root.tick_once(robot)
-        # TODO: change so this properly returns the actions intent messages
+    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)

rj_gameplay/rj_gameplay/skill/receive.py

@@ -0,0 +1,52 @@
+from abc import ABC, abstractmethod
+
+import rj_gameplay.eval as eval
+import argparse
+import py_trees
+import sys
+import time
+import numpy as np
+from typing import Optional
+
+import stp.skill as skill
+import stp.role as role
+import stp.action as action
+from rj_gameplay.action import receive, capture
+from stp.skill.action_behavior import ActionBehavior
+from stp.skill.rj_sequence import RjSequence as Sequence
+import stp.rc as rc
+from rj_msgs import msg
+
+class IReceive(skill.ISkill, ABC):
+    ...
+
+
+"""
+A skill version of receive so that actions don't have to be called in tactics
+"""
+class Receive(IReceive):
+
+    def __init__(self,
+            robot:rc.Robot = None):
+
+        self.robot = robot
+        if self.robot is not None:
+            self.receive = receive.Receive(self.robot.id)
+            self.capture = capture.Capture(self.robot.id)
+        else:
+            self.receive = receive.Receive(self.robot)
+            self.capture = capture.Capture()
+        self.receive_behavior = ActionBehavior('Receive', self.receive)
+        self.capture_behavior = ActionBehavior('Capture', self.capture)
+        self.root = Sequence('Sequence')
+        self.root.add_children([self.receive_behavior, self.capture_behavior])
+        self.root.setup_with_descendants()
+        self.__name__ = 'receive skill'
+
+    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)
+        return actions
+
+    def is_done(self, world_state:rc.WorldState):
+        return self.capture.is_done(world_state)