Merge pull request #1671 from RoboJackets/basic_defense

Basic defense play

rj_gameplay/rj_gameplay/action/intercept.py

@@ -0,0 +1,38 @@
+"""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, 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
+        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
+
+        return intent
+
+    def is_done(self, world_state) -> bool:
+        if world_state.our_robots[self.robot_id].has_ball_sense:
+            return True
+        return False

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:
@@ -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

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

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
@@ -12,7 +13,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
@@ -25,11 +26,12 @@ 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, passing_tactic_play.PassPlay())
+        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.
+    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,6 +41,10 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
         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.robot_state_subs = [None] * NUM_ROBOTS
         self.robot_intent_pubs = [None] * NUM_ROBOTS
@@ -57,6 +63,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
 
@@ -102,6 +109,13 @@ def create_field(self, msg: msg.FieldDimensions) -> None:
         if msg is not None:
             self.field = conv.field_msg_to_field(msg)
 
+    def create_goalie_id(self, msg: msg.Goalie) -> None:
+        """
+        Set game_info's 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:
         """

rj_gameplay/rj_gameplay/play/basic_defense.py

@@ -0,0 +1,64 @@
+import stp.play as play
+import stp.tactic as 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
+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),
+            goalie_tactic.GoalieTactic()
+        ]
+
+        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])

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)
-

rj_gameplay/rj_gameplay/skill/intercept.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 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
+        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)

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]),
@@ -46,7 +45,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
         self.move.target_vel = self.target_vel

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
@@ -33,13 +35,15 @@ def __init__(self, robot: rc.Robot, target_point: np.array, chip: bool,
         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)
@@ -48,9 +52,15 @@ def __init__(self, robot: rc.Robot, target_point: np.array, chip: bool,
 
     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)
+        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:

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):