Wall tactic

rj_gameplay/rj_gameplay/gameplay_node.py

@@ -2,6 +2,7 @@
 from rclpy.node import Node
 
 from rj_msgs import msg
+from rj_geometry_msgs import msg as geo_msg
 import stp.rc as rc
 import stp.utils.world_state_converter as conv
 import stp.situation as situation
@@ -11,7 +12,8 @@
 from stp.global_parameters import GlobalParameterClient
 import numpy as np
 from rj_gameplay.action.move import Move
-from rj_gameplay.play import basic122 
+from rj_gameplay.play import wall_ball
+from rj_gameplay.play import line_up 
 from typing import List, Optional, Tuple
 
 NUM_ROBOTS = 16
@@ -24,7 +26,8 @@ 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, basic122.Basic122())
+        return (None, wall_ball.WallBall())
+        # return (None, line_up.LineUp())
 
 class GameplayNode(Node):
     """
@@ -35,9 +38,10 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
         rclpy.init()
         super().__init__('gameplay_node')
         self.world_state_sub = self.create_subscription(msg.WorldState, '/vision_filter/world_state', self.create_partial_world_state, 10)
-        self.field_dimenstions = self.create_subscription(msg.FieldDimensions, '/config/field_dimensions', self.create_field, 10)
+        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.robot_state_subs = [None] * NUM_ROBOTS
         self.robot_intent_pubs = [None] * NUM_ROBOTS
 
@@ -62,6 +66,9 @@ def __init__(self, play_selector: situation.IPlaySelector, world_state: Optional
             self, '/global_parameter_server')
         local_parameters.register_parameters(self)
 
+        # publish global obstacles
+        self.global_obstacles_pub = self.create_publisher(geo_msg.ShapeSet, '/planning/global_obstacles', 10)
+
         timer_period = 1/60 #seconds
         self.timer = self.create_timer(timer_period, self.gameplay_tick)
         self.gameplay = coordinator.Coordinator(play_selector)
@@ -105,13 +112,14 @@ 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:
         """
         ticks the gameplay coordinator using recent world_state
         """
+
         if self.partial_world_state is not None and self.field is not None and len(self.robot_statuses) >= NUM_ROBOTS:
             self.world_state = conv.worldstate_creator(self.partial_world_state, self.robot_statuses, self.game_info, self.field)
         else:
@@ -121,6 +129,19 @@ def gameplay_tick(self) -> None:
             intents = self.gameplay.tick(self.world_state)
             for i in range(NUM_ROBOTS):
                 self.robot_intent_pubs[i].publish(intents[i])
+
+            # create penalty_box rect
+            penalty_box = geo_msg.Rect()
+            penalty_box.pt = [geo_msg.Point(x=1.2, y=0.0), geo_msg.Point(x=-1.2, y=1.2)]
+            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)
+            penalty_box.pt = [top_left, bot_right] 
+            # print(penalty_box.pt)
+
+            # publish Rect shape to global_obstacles topic
+            global_obstacles = geo_msg.ShapeSet()
+            global_obstacles.rectangles = [penalty_box]
+            self.global_obstacles_pub.publish(global_obstacles)
         else:
             self.get_logger().warn("World state was none!")
 

rj_gameplay/rj_gameplay/play/wall_ball.py

@@ -0,0 +1,49 @@
+import stp.play as play
+import stp.tactic as tactic
+
+from rj_gameplay.tactic import wall_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 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)
+        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 = {}
+        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)
+        role_results = play.unflatten_results(flat_results)
+
+        # Get list of all skills with assigned roles from tactics
+        skill_dict = {}
+        skills = self.wall_tactic.tick(role_results[self.wall_tactic])
+        skill_dict.update(role_results[self.wall_tactic])
+
+        return (skill_dict, skills)
+
+    def is_done(self ,world_state):
+        return self.wall_tactic.is_done(world_state)
+

rj_gameplay/rj_gameplay/skill/move.py

@@ -42,11 +42,13 @@ def __init__(self,
         self.root.setup_with_descendants()
         self.__name__ = 'move skill'
 
-    def tick(self, robot: rc.Robot, world_state): #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
+        # TODO: figure out why AttributeError: 'Move object has no attribute face_point' keeps popping up (move action does have a face_point)
+        self.move.face_point = self.face_point
         actions = self.root.tick_once(self.robot, world_state)
         return actions
-        # TODO: change so this properly returns the actions intent messages
 
     def is_done(self, world_state):
         return self.move.is_done(world_state)

rj_gameplay/rj_gameplay/tactic/wall_tactic.py

@@ -0,0 +1,199 @@
+"""Tactic to build a wall between mark pt (e.g. ball) and defense pt (e.g. goal)."""
+
+from dataclasses import dataclass
+from typing import List, Optional
+from typing import Dict, Generic, List, Optional, Tuple, Type, TypeVar
+
+import stp.action as action
+import stp.rc as rc
+import stp.tactic as tactic
+import stp.role as role
+
+import rj_gameplay.eval
+import rj_gameplay.skill as skills
+from rj_gameplay.skill import move
+import stp.skill as skill
+import numpy as np
+# TODO: replace w/ global param server
+from stp.utils.constants import RobotConstants, BallConstants
+
+class wall_cost(role.CostFn):
+    """Cost function for role request.
+    """
+    def __call__(
+        self,
+        robot: rc.Robot,
+        prev_result: Optional["RoleResult"],
+        world_state: rc.WorldState
+    ) -> float:
+
+        # TODO: make closest robots form wall, rather than setting on init
+        # aka actually use this method
+        return 0.0
+
+def find_wall_pts(num_robots: int, ball_pt: np.ndarray, goal_pt: np.ndarray, world_state: rc.WorldState):
+    """Calculates num_robots points to form a wall between the ball and goal.
+    :return list of wall_pts (numpy)
+    """
+    # TODO: param server this const
+    WALL_SPACING = BallConstants.RADIUS * 1.9
+
+    # dist is slightly greater than penalty box bounds
+    box_w = world_state.field.penalty_long_dist_m
+    box_h = world_state.field.penalty_short_dist_m
+    line_w = world_state.field.line_width_m
+    DIST_FROM_DEF = RobotConstants.RADIUS + line_w + np.hypot(box_w/2, box_h)
+
+    # check if vision is up and running
+    # (if it is these points should not be equal)
+    if (ball_pt == goal_pt).all():
+        return None
+
+    # get direction vec
+    dir_vec = (ball_pt - goal_pt) / np.linalg.norm(ball_pt - goal_pt)
+    wall_vec = np.array([dir_vec[1], -dir_vec[0]])
+
+    mid_pt = goal_pt + (dir_vec * DIST_FROM_DEF)
+    wall_pts = [mid_pt]
+
+    # set wall points in middle out pattern, given wall dir vector and WALL_SPACING constant
+    wall_pts = [mid_pt]
+    for i in range(num_robots-1):
+        mult = i//2 + 1
+        delta = (mult * (2 * RobotConstants.RADIUS + WALL_SPACING)) * wall_vec 
+        if i % 2: delta = -delta
+        wall_pts.append(mid_pt + delta)
+
+    return wall_pts
+
+def match_robots_to_wall(num_robots: int, ball_pt: np.ndarray, goal_pt: np.ndarray, world_state: rc.WorldState):
+    """Matches all wall_points to a robot.
+    :return dictionary of robot_id to List(rc.Robot, pt)
+    """
+    # assignments[robot_id] = [rc.Robot, pt]
+    assignments = {}
+
+    wall_pts = find_wall_pts(num_robots, ball_pt, goal_pt, world_state)
+    if not wall_pts: 
+        # sometimes tactic is called before ball_pos is seen
+        return None
+
+    # assign every wall pt a robot to move there
+    for wall_pt in wall_pts:
+        robot = robot_to_wall_pt(wall_pt, world_state, assignments)
+        assignments[robot.id] = (robot, wall_pt)
+
+    return assignments
+
+def robot_to_wall_pt(wall_pt: np.ndarray, world_state: rc.WorldState, assignments):
+    # TODO: dict type for assignments arg?
+    """Chooses which robot to move to a specific wall pt.
+    :return closest Robot object by dist
+    """
+    # find closest robot by dist, return it
+    min_robot = None
+    min_dist = float('inf')
+    for robot in world_state.our_robots:
+        # prevent duplicate assignments
+        if robot.id in assignments: 
+            continue
+
+        robot_pt = robot.pose[0:2]
+        dist = np.linalg.norm(robot_pt - wall_pt)
+        if dist < min_dist:
+            min_dist = dist
+            min_robot = robot
+
+    return min_robot
+
+class WallTactic(tactic.ITactic):
+
+    def __init__(self, 
+            num_robots: int, 
+            ball_pt: np.ndarray = None, 
+            goal_pt: np.ndarray = None,
+        ):
+
+        self.num_robots = num_robots
+        self.ball_pt = ball_pt 
+        self.goal_pt = goal_pt
+
+        # create move SkillEntry for every robot
+        self.move_list = [
+            tactic.SkillEntry(move.Move()) 
+            for _ in range(num_robots)
+        ]
+
+        self.wall_pts = None
+        self.cost = wall_cost()
+        self.world_state = None
+        self.wall_spots = None
+
+    def compute_props(self):
+        pass
+
+    def create_request(self, **kwargs) -> role.RoleRequest:
+        """Creates a sane default RoleRequest.
+        :return: A list of size 1 of a sane default RoleRequest.
+        """
+        pass
+
+    def get_requests(
+        self, world_state: rc.WorldState, props
+    ) -> List[tactic.RoleRequests]:
+        """
+        :return: A list of role requests for move skills needed
+        """
+
+        if world_state and world_state.ball.visible:
+            # update world_state, ball pos, goal loc
+            # TODO: resolve scenario when ball_pt/goal_pt don't need to be updated every tick (e.g. manually given)
+            self.world_state = world_state
+            self.ball_pt = world_state.ball.pos
+            self.goal_pt = world_state.field.our_goal_loc
+
+            # get dict of robot to pos in wall
+            self.wall_spots = match_robots_to_wall(self.num_robots, self.ball_pt, self.goal_pt, self.world_state)
+            if self.wall_spots is not None:
+                i = 0
+                for robot, pt in self.wall_spots.values():
+                    # assign correct robots, target points to each move skill
+                    self.move_list[i].skill.robot = robot
+                    self.move_list[i].skill.target_point = pt
+                    i += 1
+
+                # make all robots face ball_pt on move
+                for move_skill_entry in self.move_list:
+                    move_skill_entry.skill.face_point = self.ball_pt
+
+        # create a RoleRequest for each SkillEntry
+        role_requests = {
+            move_skill_entry: [role.RoleRequest(role.Priority.LOW, False, self.cost)] 
+            for move_skill_entry in self.move_list
+        }
+
+        return role_requests
+
+    def tick(self, role_results: tactic.RoleResults) -> List[tactic.SkillEntry]:
+        """
+        :return: A list of skills depending on which roles are filled
+        """
+
+        # create list of skills based on if RoleResult exists for SkillEntry 
+        skills = [
+            move_skill_entry
+            for move_skill_entry in self.move_list
+            if role_results[move_skill_entry][0]
+        ]
+
+        return skills
+
+    def is_done(self, world_state):
+        """
+        :return boolean indicating if tactic is done
+        """
+        for move_skill in self.move_list:
+            if not move_skill.skill.is_done(world_state):
+                return False
+        return True
+