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

@@ -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/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
@@ -48,15 +54,15 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
 
         for i in range(NUM_ROBOTS):
             self.robot_state_subs[i] = self.create_subscription(msg.RobotStatus, '/radio/robot_status/robot_'+str(i), self.create_partial_robots, 10)
- 
+
         for i in range(NUM_ROBOTS):
             self.robot_intent_pubs[i] = self.create_publisher(msg.RobotIntent, '/gameplay/robot_intent/robot_'+str(i), 10)
 
-
         self.get_logger().info("Gameplay node started")
         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
 
@@ -65,7 +71,8 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
         local_parameters.register_parameters(self)
 
         # publish global obstacles
-        self.global_obstacles_pub = self.create_publisher(geo_msg.ShapeSet, '/planning/global_obstacles', 10)
+        self.goal_zone_obstacles_pub = self.create_publisher(
+            geo_msg.ShapeSet, '/planning/goal_zone_obstacles', 10)
 
         timer_period = 1/60 #seconds
         self.timer = self.create_timer(timer_period, self.gameplay_tick)
@@ -87,7 +94,7 @@ def create_partial_robots(self, msg: msg.RobotStatus) -> None:
             robot = conv.robotstatus_to_partial_robot(msg)
             index = robot.robot_id
             self.robot_statuses[index] = robot
-        
+
     def create_game_info(self, msg: msg.GameState) -> None:
         """
         Create game info object from Game State message
@@ -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:
         """
@@ -110,7 +124,7 @@ def get_world_state(self) -> rc.WorldState:
         if self.partial_world_state is not None and self.field is not None and len(self.robot_statuses) == len(self.partial_world_state.our_robots):
 
             self.world_state = conv.worldstate_creator(self.partial_world_state, self.robot_statuses, self.game_info, self.field)
-        
+
         return self.world_state
 
     def gameplay_tick(self) -> None:
@@ -132,21 +146,21 @@ def gameplay_tick(self) -> None:
             our_penalty = geo_msg.Rect()
             top_left = geo_msg.Point(x=self.field.penalty_long_dist_m/2 + self.field.line_width_m, y=0.0)
             bot_right = geo_msg.Point(x=-self.field.penalty_long_dist_m/2 - self.field.line_width_m, y=self.field.penalty_short_dist_m)
-            our_penalty.pt = [top_left, bot_right] 
+            our_penalty.pt = [top_left, bot_right]
 
             # create their_penalty rect
             their_penalty = geo_msg.Rect()
             bot_left = geo_msg.Point(x=self.field.penalty_long_dist_m/2 + self.field.line_width_m, y=self.field.length_m)
             top_right = geo_msg.Point(x=-self.field.penalty_long_dist_m/2 - self.field.line_width_m, y=self.field.length_m - self.field.penalty_short_dist_m)
-            their_penalty.pt = [bot_left, top_right] 
+            their_penalty.pt = [bot_left, top_right]
 
-            # publish Rect shape to global_obstacles topic
-            global_obstacles = geo_msg.ShapeSet()
-            global_obstacles.rectangles = [our_penalty, their_penalty]
-            self.global_obstacles_pub.publish(global_obstacles)
+            # publish Rect shape to goal_zone_obstacles topic
+            goal_zone_obstacles = geo_msg.ShapeSet()
+            goal_zone_obstacles.rectangles = [our_penalty, their_penalty]
+            self.goal_zone_obstacles_pub.publish(goal_zone_obstacles)
         else:
             self.get_logger().warn("World state was none!")
-    
+
     def tick_override_actions(self, world_state) -> None:
         for i in range(0,NUM_ROBOTS):
             if self.override_actions[i] is not None:

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.face_point = self.face_point