diff --git a/rj_gameplay/rj_gameplay/action/intercept.py b/rj_gameplay/rj_gameplay/action/intercept.py new file mode 100644 index 0000000000..b2152b5891 --- /dev/null +++ b/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 diff --git a/rj_gameplay/rj_gameplay/action/kick.py b/rj_gameplay/rj_gameplay/action/kick.py index 8ef8005fda..fa60ab590a 100644 --- a/rj_gameplay/rj_gameplay/action/kick.py +++ b/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 diff --git a/rj_gameplay/rj_gameplay/action/pivot.py b/rj_gameplay/rj_gameplay/action/pivot.py index c65e4378c2..de5e569b9e 100644 --- a/rj_gameplay/rj_gameplay/action/pivot.py +++ b/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 diff --git a/rj_gameplay/rj_gameplay/gameplay_node.py b/rj_gameplay/rj_gameplay/gameplay_node.py index b0d1e4a5e8..b33e17717a 100644 --- a/rj_gameplay/rj_gameplay/gameplay_node.py +++ b/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: """ diff --git a/rj_gameplay/rj_gameplay/play/basic_defense.py b/rj_gameplay/rj_gameplay/play/basic_defense.py new file mode 100644 index 0000000000..8715a0bc39 --- /dev/null +++ b/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]) diff --git a/rj_gameplay/rj_gameplay/play/wall_ball.py b/rj_gameplay/rj_gameplay/play/wall_ball.py index 0c008e6a6b..12e2eef4ad 100644 --- a/rj_gameplay/rj_gameplay/play/wall_ball.py +++ b/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) - diff --git a/rj_gameplay/rj_gameplay/skill/intercept.py b/rj_gameplay/rj_gameplay/skill/intercept.py new file mode 100644 index 0000000000..2db6d385df --- /dev/null +++ b/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) diff --git a/rj_gameplay/rj_gameplay/skill/move.py b/rj_gameplay/rj_gameplay/skill/move.py index dbe937273d..6f0413f52e 100644 --- a/rj_gameplay/rj_gameplay/skill/move.py +++ b/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 diff --git a/rj_gameplay/rj_gameplay/skill/pivot_kick.py b/rj_gameplay/rj_gameplay/skill/pivot_kick.py index 25c073adbc..e0f419fe52 100644 --- a/rj_gameplay/rj_gameplay/skill/pivot_kick.py +++ b/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: diff --git a/rj_gameplay/rj_gameplay/skill/receive.py b/rj_gameplay/rj_gameplay/skill/receive.py index 71deabf079..6c39767309 100644 --- a/rj_gameplay/rj_gameplay/skill/receive.py +++ b/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): diff --git a/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py new file mode 100644 index 0000000000..62265735c0 --- /dev/null +++ b/rj_gameplay/rj_gameplay/tactic/goalie_tactic.py @@ -0,0 +1,176 @@ +"""Tactic to produce goalie behavior, which tracks the ball, moves to block if a shot on goal is taken, and stays within the goalie box (generally).""" + +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, receive, pivot_kick #, intercept +import stp.skill as skill +import numpy as np +# TODO: replace w/ global param server +from stp.utils.constants import RobotConstants, BallConstants +import stp.global_parameters as global_parameters +from stp.local_parameters import Param + +# TODO: param server this const +MIN_WALL_RAD = 0 +GOALIE_PCT_TO_BALL = 0.15 + + +class GoalieCost(role.CostFn): + """Cost function for role request. Want only the designated goalie to be selected. + """ + def __call__(self, robot: rc.Robot, prev_result: Optional["RoleResult"], + world_state: rc.WorldState) -> float: + + if world_state.game_info is not None: + if robot.id == world_state.game_info.goalie_id: + return -1.0 + return 999.0 + + +def get_goalie_pt(world_state: rc.WorldState) -> np.ndarray: + """Finds point for goalie to best be in to block a shot. + :return numpy point + """ + # TODO: param server any constant from stp/utils/constants.py (this includes BallConstants) + ball_pt = world_state.ball.pos + goal_pt = world_state.field.our_goal_loc + + dir_vec = (ball_pt - goal_pt) / np.linalg.norm(ball_pt - goal_pt) + # get in-between ball and goal, staying behind wall + dist_from_goal = min( + GOALIE_PCT_TO_BALL * np.linalg.norm(ball_pt - goal_pt), + MIN_WALL_RAD - RobotConstants.RADIUS * 2.1) + mid_pt = goal_pt + (dir_vec * dist_from_goal) + return mid_pt + + +def get_block_pt(world_state: rc.WorldState, my_pos: np.ndarray) -> np.ndarray: + pos = world_state.ball.pos + vel = world_state.ball.vel + + block_pt = np.array([(my_pos[1] - pos[1]) / vel[1] * vel[0] + pos[0], + my_pos[1]]) + + return block_pt + + +class GoalieTactic(tactic.ITactic): + def __init__(self): + + # init skills + self.move_se = tactic.SkillEntry(move.Move()) + self.receive_se = tactic.SkillEntry(receive.Receive()) + self.pivot_kick_se = tactic.SkillEntry(pivot_kick.PivotKick(None, target_point = np.array([0.0, 6.0]), chip=True, kick_speed=6.0)) + + # TODO: rename cost_list to role_cost in other gameplay files + self.role_cost = GoalieCost() + + 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]: + global MIN_WALL_RAD + """ + :return: A list of role requests for move skills needed + """ + + # TODO: this calculation is copy-pasted from wall_tactic + # put into common param file: https://www.geeksforgeeks.org/global-keyword-in-python/ + + # 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 + MIN_WALL_RAD = RobotConstants.RADIUS + line_w + np.hypot( + box_w / 2, box_h) + + role_requests = {} + if world_state and world_state.ball.visible: + ball_speed = np.linalg.norm(world_state.ball.vel) + ball_dist = np.linalg.norm(world_state.field.our_goal_loc - + world_state.ball.pos) + + if ball_speed < 1.0 and ball_dist < MIN_WALL_RAD - RobotConstants.RADIUS * 2.1: + self.move_se = tactic.SkillEntry(move.Move()) + if not self.receive_se.skill.is_done(world_state): + # if ball is slow and inside goalie box, collect it + role_requests[self.receive_se] = [ + role.RoleRequest(role.Priority.HIGH, True, self.role_cost) + ] + else: + # if ball has been stopped already, chip toward center field + self.pivot_kick_se.skill.target_point = np.array([0.0, 6.0]) + role_requests[self.pivot_kick_se] = [ + role.RoleRequest(role.Priority.HIGH, True, self.role_cost) + ] + else: + ball_to_goal_time = ball_dist / ball_speed + if ball_speed > 0 and ball_to_goal_time < 2: + # if ball is moving and coming at goal, move laterally to block ball + # TODO (#1676): replace this logic with a real intercept planner + self.move_se.skill.target_point = get_block_pt( + world_state, get_goalie_pt(world_state)) + self.move_se.skill.face_point = world_state.ball.pos + role_requests[self.move_se] = [ + role.RoleRequest(role.Priority.HIGH, True, + self.role_cost) + ] + else: + # else, track ball normally + self.move_se.skill.target_point = get_goalie_pt( + world_state) + self.move_se.skill.face_point = world_state.ball.pos + role_requests[self.move_se] = [ + role.RoleRequest(role.Priority.HIGH, True, + self.role_cost) + ] + if self.pivot_kick_se.skill.is_done(world_state): + self.pivot_kick_se = tactic.SkillEntry(pivot_kick.PivotKick(None, target_point = np.array([0.0, 6.0]), chip=True, kick_speed=4.0)) + + 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_result = role_results[self.move_se] + receive_result = role_results[self.receive_se] + pivot_kick_result = role_results[self.pivot_kick_se] + + # move skill takes priority + if move_result and move_result[0].is_filled(): + skills.append(self.move_se) + elif receive_result and receive_result[0].is_filled(): + skills.append(self.receive_se) + elif pivot_kick_result and pivot_kick_result[0].is_filled(): + skills.append(self.pivot_kick_se) + + return skills + + def is_done(self, world_state): + """ + :return boolean indicating if tactic is done + """ + # goalie tactic always active + return False diff --git a/rj_gameplay/rj_gameplay/tactic/wall_tactic.py b/rj_gameplay/rj_gameplay/tactic/wall_tactic.py index d889eee1b5..518507c1d4 100644 --- a/rj_gameplay/rj_gameplay/tactic/wall_tactic.py +++ b/rj_gameplay/rj_gameplay/tactic/wall_tactic.py @@ -18,30 +18,33 @@ from stp.utils.constants import RobotConstants, BallConstants import stp.global_parameters as global_parameters +MIN_WALL_RAD = None + + class wall_cost(role.CostFn): """Cost function for role request. """ - def __init__(self, wall_pt: np.ndarray=None): + def __init__(self, wall_pt: np.ndarray = None): self.wall_pt = wall_pt - def __call__( - self, - robot: rc.Robot, - prev_result: Optional["RoleResult"], - world_state: rc.WorldState - ) -> float: + def __call__(self, robot: rc.Robot, prev_result: Optional["RoleResult"], + world_state: rc.WorldState) -> float: if robot is None or self.wall_pt is None: return 0 # TODO(#1669): Remove this once role assignment no longer assigns non-visible robots if not robot.visible: - return 99999 # float('inf') threw ValueError + return 99999 # float('inf') threw ValueError # costs should be in seconds, not dist - return np.linalg.norm(robot.pose[0:2]-self.wall_pt) / global_parameters.soccer.robot.max_speed + return np.linalg.norm(robot.pose[0:2] - self.wall_pt + ) / global_parameters.soccer.robot.max_speed + -def find_wall_pts(num_wallers: int, world_state: rc.WorldState) -> List[np.ndarray]: +def find_wall_pts(num_wallers: int, + world_state: rc.WorldState) -> List[np.ndarray]: + global MIN_WALL_RAD """Calculates num_wallers points to form a wall between the ball and goal. :return list of wall_pts (as numpy arrays) """ @@ -56,44 +59,40 @@ def find_wall_pts(num_wallers: int, world_state: rc.WorldState) -> List[np.ndarr 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) + MIN_WALL_RAD = RobotConstants.RADIUS + line_w + np.hypot(box_w / 2, box_h) # 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]]) # find mid_pt - mid_pt = goal_pt + (dir_vec * DIST_FROM_DEF) + mid_pt = goal_pt + (dir_vec * MIN_WALL_RAD) 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_wallers-1): - mult = i//2 + 1 - delta = (mult * (2 * RobotConstants.RADIUS + WALL_SPACING)) * wall_vec + for i in range(num_wallers - 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 -class WallTactic(tactic.ITactic): +class WallTactic(tactic.ITactic): def __init__(self, num_wallers: int): self.num_wallers = num_wallers # create move SkillEntry for every robot self.move_list = [ - tactic.SkillEntry(move.Move()) - for _ in range(num_wallers) + tactic.SkillEntry(move.Move()) for _ in range(num_wallers) ] # create empty cost_list (filled in get_requests) - self.cost_list = [ - wall_cost() - for _ in range(self.num_wallers) - ] - + self.cost_list = [wall_cost() for _ in range(self.num_wallers)] + def compute_props(self): pass @@ -103,9 +102,8 @@ def create_request(self, **kwargs) -> role.RoleRequest: """ pass - def get_requests( - self, world_state: rc.WorldState, props - ) -> List[tactic.RoleRequests]: + def get_requests(self, world_state: rc.WorldState, + props) -> List[tactic.RoleRequests]: """ :return: A list of role requests for move skills needed """ @@ -121,28 +119,25 @@ def get_requests( # create RoleRequest for each SkillEntry role_requests = { - self.move_list[i]: [role.RoleRequest(role.Priority.HIGH, False, self.cost_list[i])] + self.move_list[i]: + [role.RoleRequest(role.Priority.HIGH, False, self.cost_list[i])] for i in range(self.num_wallers) } - for se, rr in role_requests.items(): - print(se.skill.robot) - # print(rr) - return role_requests - def tick(self, role_results: tactic.RoleResults) -> List[tactic.SkillEntry]: + 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 + # create list of skills based on if RoleResult exists for SkillEntry skills = [ - move_skill_entry - for move_skill_entry in self.move_list + 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): diff --git a/rj_gameplay/stp/rc.py b/rj_gameplay/stp/rc.py index 2650ca3d9e..e51e4b2609 100644 --- a/rj_gameplay/stp/rc.py +++ b/rj_gameplay/stp/rc.py @@ -425,19 +425,23 @@ def goal_height_m(self) -> float: class GameInfo: """State of the soccer game""" - __slots__ = ["__period", "__state", "__restart", "__our_restart"] + __slots__ = [ + "__period", "__state", "__restart", "__our_restart", "__goalie_id" + ] __period: GamePeriod __state: GameState __restart: GameRestart __our_restart: bool + __goalie_id: int def __init__(self, period: GamePeriod, state: GameState, - restart: GameRestart, our_restart: bool): + restart: GameRestart, our_restart: bool, goalie_id: int): self.__period = period self.__state = state self.__restart = restart self.__our_restart = our_restart + self.__goalie_id = goalie_id @property def period(self) -> GamePeriod: @@ -473,6 +477,18 @@ def our_restart(self) -> bool: return self.__our_restart + @property + def goalie_id(self) -> int: + """ + :return: id (int) of our designated goalie + """ + return self.__goalie_id + + def set_goalie_id(self, goalie_id: int) -> None: + """Sets goalie id. (Created to maintain private data convention.) + """ + self.__goalie_id = goalie_id + def is_restart(self) -> bool: """ :return: True if there is a restart. diff --git a/rj_gameplay/stp/utils/world_state_converter.py b/rj_gameplay/stp/utils/world_state_converter.py index ed74cfac59..7d57029abb 100644 --- a/rj_gameplay/stp/utils/world_state_converter.py +++ b/rj_gameplay/stp/utils/world_state_converter.py @@ -26,7 +26,7 @@ class RobotStatus(): def __init__(self, robot_id: RobotId = None, has_ball_sense: bool = None, kicker_charged: bool = None, kicker_healthy: bool = None, lethal_fault: bool = None): - + self.robot_id = robot_id self.has_ball_sense = has_ball_sense self.kicker_charged = kicker_charged @@ -130,8 +130,7 @@ def ballstate_to_ball(ball_msg: msg.BallState) -> rc.Ball: vel = np.array([dx,dy]) visible = ball_msg.visible - - ball = rc.Ball(pos,vel, visible) + ball = rc.Ball(pos, vel, visible) return ball @@ -141,14 +140,12 @@ def gamestate_to_gameinfo(game_state_msg: msg.GameState) -> rc.GameInfo: """ period = game_state_msg.period - state = game_state_msg.state - restart = game_state_msg.restart - our_restart = game_state_msg.our_restart - game_info = rc.GameInfo(period, state, restart, our_restart) + game_info = rc.GameInfo(period, state, restart, our_restart, + None) # goalie_id set later return game_info @@ -255,4 +252,3 @@ def worldstate_creator(partial_world_state: PartialWorldState, robot_statuses: L world_state = rc.WorldState(our_robots, their_robots, partial_world_state.ball, game_info, field) return world_state - diff --git a/soccer/src/soccer/referee/referee_base.cpp b/soccer/src/soccer/referee/referee_base.cpp index 200161791a..f6808f24a4 100644 --- a/soccer/src/soccer/referee/referee_base.cpp +++ b/soccer/src/soccer/referee/referee_base.cpp @@ -104,9 +104,12 @@ void RefereeBase::send() { } if (!goalie_valid_) { - GoalieMsg msg; - msg.goalie_id = blue_team_ ? blue_info_.goalie : yellow_info_.goalie; - goalie_id_pub_->publish(msg); + goalie_msg.goalie_id = blue_team_ ? blue_info_.goalie : yellow_info_.goalie; + goalie_id_pub_->publish(goalie_msg); + // publish the goalie_id on a timer, since transient_local QoS isn't working + // TODO (#1675): fix transient_local OR publish all similar msgs on timer + pub_timer_ = this->create_wall_timer(std::chrono::seconds(1), + [this]() { goalie_id_pub_->publish(goalie_msg); }); goalie_valid_ = true; } @@ -145,4 +148,4 @@ void RefereeBase::update_team_color_from_names() { } } -} // namespace referee \ No newline at end of file +} // namespace referee diff --git a/soccer/src/soccer/referee/referee_base.hpp b/soccer/src/soccer/referee/referee_base.hpp index 5ad4e63c6c..a905cfdb27 100644 --- a/soccer/src/soccer/referee/referee_base.hpp +++ b/soccer/src/soccer/referee/referee_base.hpp @@ -162,6 +162,11 @@ class RefereeBase : public rclcpp::Node { rclcpp::Publisher::SharedPtr their_team_info_pub_; rclcpp::Publisher::SharedPtr game_state_pub_; + // setup to publish goalie_msg on a timer, as it's only exposed on + // gameplay start/halts and transient_local QoS is failing us + GoalieMsg goalie_msg; + rclcpp::TimerBase::SharedPtr pub_timer_; + /** * @brief Update the team color from the names currently available in the * blue and yellow team information. @@ -174,4 +179,4 @@ class RefereeBase : public rclcpp::Node { params::LocalROS2ParamProvider param_provider_; }; -} // namespace referee \ No newline at end of file +} // namespace referee