diff --git a/rj_gameplay/rj_gameplay/gameplay_node.py b/rj_gameplay/rj_gameplay/gameplay_node.py index 46704bd54bc..6526507d441 100644 --- a/rj_gameplay/rj_gameplay/gameplay_node.py +++ b/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 +from rj_gameplay.play import defensive_clear from typing import List, Optional, Tuple NUM_ROBOTS = 16 diff --git a/rj_gameplay/rj_gameplay/play/defensive_clear.py b/rj_gameplay/rj_gameplay/play/defensive_clear.py new file mode 100644 index 00000000000..cb76d22ec0a --- /dev/null +++ b/rj_gameplay/rj_gameplay/play/defensive_clear.py @@ -0,0 +1,50 @@ +import stp.play as play +import stp.tactic as tactic + +from rj_gameplay.tactic import stub_striker, nmark_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 DefensiveClear(play.IPlay): + + def __init__(self): + # TODO: add chipper tactic here + + self.two_mark = nmark_tactic.NMarkTactic(2) + 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.striker_tactic] = self.striker_tactic.get_requests(world_state, None) + role_requests[self.two_mark] = (self.two_mark.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 + + # skills = self.striker_tactic.tick(role_results[self.striker_tactic]) + self.two_mark.tick(role_results[self.two_mark]) + skills = self.two_mark.tick(role_results[self.two_mark]) + skill_dict = {} + # skill_dict.update(role_results[self.striker_tactic]) + skill_dict.update(role_results[self.two_mark]) + + return (skill_dict, skills) + + def is_done(self, world_state): + return self.two_mark.is_done(world_state) diff --git a/rj_gameplay/rj_gameplay/skill/mark.py b/rj_gameplay/rj_gameplay/skill/mark.py index 3cb1d021eaa..687b291def3 100644 --- a/rj_gameplay/rj_gameplay/skill/mark.py +++ b/rj_gameplay/rj_gameplay/skill/mark.py @@ -53,9 +53,7 @@ class IMark(skill.ISkill, ABC): """ class Mark(IMark): - def __init__(self, - robot : rc.Robot = None, - target_robot : rc.Robot = None): + def __init__(self, robot: rc.Robot = None, target_robot: rc.Robot = None) -> None: self.__name__ = 'Mark Skill' self.robot = robot diff --git a/rj_gameplay/rj_gameplay/tactic/nmark_tactic.py b/rj_gameplay/rj_gameplay/tactic/nmark_tactic.py new file mode 100644 index 00000000000..13971d49e76 --- /dev/null +++ b/rj_gameplay/rj_gameplay/tactic/nmark_tactic.py @@ -0,0 +1,117 @@ +from dataclasses import dataclass +from typing import List, Optional + +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 mark +import stp.skill as skill + +import numpy as np + +import stp.global_parameters as global_parameters + +def get_closest_enemies_to_ball(num_enemies: int, world_state: rc.WorldState) -> List[rc.Robot]: + ball_pt = world_state.ball.pos + + dist_to_enemies = { + np.linalg.norm(ball_pt - robot.pose[0:2]): robot + for robot in world_state.their_robots + } + + # sort dict keys by dist (shortest first) + # return enemies that correspond to n shortest dists + return [dist_to_enemies[dist] for dist in sorted(dist_to_enemies.keys())[0:num_enemies]] + +class marker_cost(role.CostFn): + """Pick mark robots based on dist to the ball point + """ + def __init__(self, enemy_to_mark: rc.Robot=None): + self.enemy_to_mark = enemy_to_mark + + def __call__( + self, + robot: rc.Robot, + prev_result: Optional["RoleResult"], + world_state: rc.WorldState, + ) -> float: + + # TODO: prevent gameplay crashing w/out this check + if robot is None or self.enemy_to_mark is None: + return 0 + + return np.linalg.norm(robot.pose[0:2]-self.enemy_to_mark.pose[0:2]) / global_parameters.soccer.robot.max_speed + +class NMarkTactic(tactic.ITactic): + """Marks the n closest enemies to ball with the closest robots on our team to said enemies. + """ + def __init__(self, n: int): + self.num_markers = n + + # create empty mark SkillEntry for each robot + self.mark_list = [ + tactic.SkillEntry(mark.Mark()) + for i in range(self.num_markers) + ] + + # create cost func for each robot + self.cost_list = [ + marker_cost() + for _ in self.mark_list + ] + + 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: role request for n markers + """ + + if world_state is not None and world_state.ball.visible: + # assign n closest enemies to respective skill and role costFn + closest_enemies = get_closest_enemies_to_ball(self.num_markers, world_state) + for i in range(self.num_markers): + self.mark_list[i].skill.target_robot = closest_enemies[i] + self.cost_list[i].enemy_to_mark = closest_enemies[i] + + # create RoleRequest for each SkillEntry + role_requests = { + self.mark_list[i]: [role.RoleRequest(role.Priority.LOW, False, self.cost_list[i])] + for i in range(self.num_markers) + } + + return role_requests + + def tick(self, role_results: tactic.RoleResults) -> List[tactic.SkillEntry]: + """ + :return: skills for the number of markers assigned from the n markers + """ + + # create list of skills based on if RoleResult exists for SkillEntry + skills = [ + mark_skill_entry + for mark_skill_entry in self.mark_list + if role_results[mark_skill_entry][0] + ] + + return skills + + def is_done(self, world_state): + # TODO: replace all similar is_done() with a .all() and generator expr + # see https://www.w3schools.com/python/ref_func_all.asp + for mark_skill in self.mark_list: + if not mark_skill.skill.is_done(world_state): + return False + return True diff --git a/rj_gameplay/rj_gameplay/tactic/stub_nmark.py b/rj_gameplay/rj_gameplay/tactic/stub_nmark.py deleted file mode 100644 index 846c76a2822..00000000000 --- a/rj_gameplay/rj_gameplay/tactic/stub_nmark.py +++ /dev/null @@ -1,87 +0,0 @@ -"""Contains the stub for the mark tactic. """ - -from dataclasses import dataclass -from typing import List, Optional - -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 mark -import stp.skill as skill - -import numpy as np - -class marker_cost(role.CostFn): - """ - A cost function for how to choose a marker - TODO: Implement a better cost function - """ - - def __call__( - self, - robot: rc.Robot, - prev_result: Optional["RoleResult"], - world_state: rc.WorldState, - ) -> float: - - return robot.pose[1] - world_state.ball.pos[1] - -def marker_heuristic(point: np.array): - return 1 - -class NMark(tactic.ITactic): - """ - A tactic which creates n robots with some marking heuristic - """ - def __init__(self, n: int): - self.num_markers = n - self.markers_list = [] - for i in range(self.num_markers): - self.markers_list.append(tactic.SkillEntry(mark.Mark(None, None))) - self.cost = marker_cost() - - 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: role request for n markers - """ - - role_requests = {} - - for i in range(self.num_markers): - role_requests[self.markers_list[i]] = [role.RoleRequest(role.Priority.LOW, False, self.cost)] - - return role_requests - - def tick(self, role_results: tactic.RoleResults) -> List[tactic.SkillEntry]: - """ - :return: skills for the number of markers assigned from the n markers - """ - skills = [] - - for i in range(self.num_markers): - if role_results[self.markers_list[i]][0]: - skills.append(self.markers_list[i]) - - return skills - - def is_done(self, world_state): - for mark_skill in self.markers_list: - if not mark_skill.skill.is_done(world_state): - return False - return True