diff --git a/game_controller_hsl/config/game_controller_settings.yaml b/game_controller_hsl/config/game_controller_settings.yaml index 7f8c2c8..1e91348 100644 --- a/game_controller_hsl/config/game_controller_settings.yaml +++ b/game_controller_hsl/config/game_controller_settings.yaml @@ -6,3 +6,4 @@ game_controller_hsl: listen_host: '0.0.0.0' listen_port: 3838 answer_port: 3939 + lost_timeout: 5 diff --git a/game_controller_hsl/game_controller_hsl/data/__init__.py b/game_controller_hsl/game_controller_hsl/data/__init__.py new file mode 100644 index 0000000..4820389 --- /dev/null +++ b/game_controller_hsl/game_controller_hsl/data/__init__.py @@ -0,0 +1,2 @@ +from game_controller_hsl.data.game_control_data import GameControlDataStruct +from game_controller_hsl.data.game_control_return_data import GameControlReturnDataStruct diff --git a/game_controller_hsl/game_controller_hsl/data/game_control_data.py b/game_controller_hsl/game_controller_hsl/data/game_control_data.py new file mode 100644 index 0000000..5884f6a --- /dev/null +++ b/game_controller_hsl/game_controller_hsl/data/game_control_data.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python +# -*- coding:utf-8 -*- + +from construct import ( + Array, + Byte, + Const, + Enum, + Flag, + Int16sl, + Int16ul, + Struct, +) + +GAME_CONTROLLER_STRUCT_VERSION = 20 +GAME_CONTROLLER_STRUCT_HEADER = b"RGme" + +MAX_NUM_PLAYERS = 20 + +Short = Int16ul + +RobotInfoStruct = Struct( + "penalty" / Enum( + Byte, + PENALTY_NONE=0, + PENALTY_ILLEGAL_POSITIONING=1, + PENALTY_MOTION_IN_SET=2, + PENALTY_MOTION_IN_STOP=3, + PENALTY_LOCAL_GAME_STUCK=4, + PENALTY_INCAPABLE_ROBOT=5, + PENALTY_PICK_UP=6, + PENALTY_BALL_HOLDING=7, + PENALTY_LEAVING_THE_FIELD=8, + PENALTY_PLAYING_WITH_ARMS_HANDS=9, + PENALTY_PUSHING=10, + PENALTY_CAUTIONED=11, + PENALTY_SENT_OFF=12, + PENALTY_SUBSTITUTE=13, + ), + "secs_till_unpenalized" / Byte, + "cautions" / Byte, +) + +TeamInfoStruct = Struct( + "team_number" / Byte, + "field_player_color" + / Enum( + Byte, + BLUE=0, + RED=1, + YELLOW=2, + BLACK=3, + WHITE=4, + GREEN=5, + ORANGE=6, + PURPLE=7, + BROWN=8, + GRAY=9, + ), + "goalkeeper_color" + / Enum( + Byte, + BLUE=0, + RED=1, + YELLOW=2, + BLACK=3, + WHITE=4, + GREEN=5, + ORANGE=6, + PURPLE=7, + BROWN=8, + GRAY=9, + ), + "goalkeeper" / Byte, + "score" / Byte, + "penalty_shot" / Byte, # penalty shot counter + "single_shots" / Short, # bits represent penalty shot success + "message_budget" / Short, # number of team messages the team is allowed to send for the remainder of the game + "players" / Array(MAX_NUM_PLAYERS, RobotInfoStruct), +) + +GameControlDataStruct = Struct( + "header" / Const(GAME_CONTROLLER_STRUCT_HEADER), + "version" / Const(GAME_CONTROLLER_STRUCT_VERSION, Byte), + "packet_number" / Byte, + "players_per_team" / Byte, + "competition_type" + / Enum( + Byte, + COMPETITION_TYPE_SMALL=0, + COMPETITION_TYPE_MIDDLE=1, + COMPETITION_TYPE_LARGE=2, + ), + "stopped" / Flag, + "game_phase" + / Enum( + Byte, + GAME_PHASE_NORMAL=0, + GAME_PHASE_PENALTY_SHOOT_OUT=1, + GAME_PHASE_EXTRA_TIME=2, + GAME_PHASE_TIMEOUT=3, + ), + "state" + / Enum( + Byte, + STATE_INITIAL=0, + STATE_READY=1, # Go to (starting) position + STATE_SET=2, # Hold position (no motion allowed) + STATE_PLAYING=3, # Normal play + STATE_FINISHED=4, # Game over + ), + "set_play" + / Enum( + Byte, + SET_PLAY_NONE=0, + SET_PLAY_DIRECT_FREE_KICK=1, + SET_PLAY_INDIRECT_FREE_KICK=2, + SET_PLAY_PENALTY_KICK=3, + SET_PLAY_THROW_IN=4, + SET_PLAY_GOAL_KICK=5, + SET_PLAY_CORNER_KICK=6, + ), + "first_half" / Flag, + "kicking_team" / Byte, + "secs_remaining" / Int16sl, + "secondary_time" / Int16sl, + "teams" / Array(2, TeamInfoStruct), +) diff --git a/game_controller_hsl/game_controller_hsl/data/game_control_return_data.py b/game_controller_hsl/game_controller_hsl/data/game_control_return_data.py new file mode 100644 index 0000000..daa9a87 --- /dev/null +++ b/game_controller_hsl/game_controller_hsl/data/game_control_return_data.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python +from construct import ( + Byte, + Const, + Flag, + Float32l, + Struct, +) + +GAME_CONTROLLER_RETURN_STRUCT_VERSION = 4 +GAME_CONTROLLER_RETURN_STRUCT_HEADER = b"RGrt" + +GameControlReturnDataStruct = Struct( + "header" / Const(GAME_CONTROLLER_RETURN_STRUCT_HEADER), + "version" / Const(GAME_CONTROLLER_RETURN_STRUCT_VERSION, Byte), + "player_number" / Byte, + "team_number" / Byte, + "fallen" / Flag, + "pose" / Float32l[3], + "ball_age" / Float32l, + "ball" / Float32l[2], +) diff --git a/game_controller_hsl/game_controller_hsl/receiver.py b/game_controller_hsl/game_controller_hsl/receiver.py index 86a419d..7eb243c 100755 --- a/game_controller_hsl/game_controller_hsl/receiver.py +++ b/game_controller_hsl/game_controller_hsl/receiver.py @@ -13,30 +13,38 @@ # limitations under the License. import socket + import rclpy -from construct import ConstError +from ros2_numpy import numpify + +from construct import ConstError, Container +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus +from rclpy.duration import Duration from rclpy.node import Node from rclpy.time import Time -from rclpy.duration import Duration from std_msgs.msg import Header -from construct import Container -from geometry_msgs.msg import PointStamped -from std_msgs.msg import Float32, Bool -from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus -from game_controller_hsl.gamestate import GameStateStruct, ResponseStruct + +from game_controller_hsl.data import GameControlDataStruct, GameControlReturnDataStruct from game_controller_hsl.utils import get_parameters_from_other_node -from game_controller_hsl_interfaces.msg import GameState, PlayerStatusPose +from game_controller_hsl_interfaces.msg import GameState, PlayerStateResponse + +from tf_transformations import euler_from_quaternion class GameStateReceiver(Node): - """This class puts up a simple UDP Server which receives the - *addr* parameter to listen to the packages from the game_controller. + """ + This class provides a simple UDP Server which listens to packages from the game_controller on *listen_host* and *listen_port* + and respond with the robots state on *answer_port*. - If it receives a package it will be interpreted with the construct data - structure and the :func:`on_new_gamestate` will be called with the content. + If it receives a package it will be interpreted with the construct data structure and the :func:`build_game_state_msg` + will be called with the content to build a ros message of type :class:`game_controller_hsl_interfaces.msg.GameState` + which is then published on the topic */gamestate*. - After this we send a package back to the GC""" + On a received ros message of type :class:`game_controller_hsl_interfaces.msg.PlayerStateResponse` on topic + */player_state_response* the current state is updated and will be sent back to the game controller on the next + received package. + """ def __init__(self, *args, **kwargs): super().__init__( @@ -51,125 +59,100 @@ def __init__(self, *args, **kwargs): if self.has_parameter("team_id") and self.has_parameter("bot_id"): self.get_logger().info("Found team_id and bot_id parameter, using them") # Get the parameters from our node - self.team_number = self.get_parameter("team_id").value - self.player_number = self.get_parameter("bot_id").value + self.team_number: int = self.get_parameter("team_id").get_parameter_value().integer_value + self.player_number: int = self.get_parameter("bot_id").get_parameter_value().integer_value else: self.get_logger().info( "No team_id and bot_id parameter set in game_controller, getting them from blackboard" ) # Get the parameter names from the parameter server - param_blackboard_name: str = self.get_parameter("parameter_blackboard_name").value - team_id_param_name: str = self.get_parameter("team_id_param_name").value - bot_id_param_name: str = self.get_parameter("bot_id_param_name").value - # Get the parameters from the blackboard + param_blackboard_name: str = self.get_parameter("parameter_blackboard_name").get_parameter_value().string_value + team_id_param_name: str = self.get_parameter("team_id_param_name").get_parameter_value().string_value + bot_id_param_name: str = self.get_parameter("bot_id_param_name").get_parameter_value().string_value + params = get_parameters_from_other_node( self, param_blackboard_name, [team_id_param_name, bot_id_param_name] ) - # Set the parameters self.team_number = params[team_id_param_name] self.player_number = params[bot_id_param_name] - self.is_fallen: bool = False - self.ball_age: float = 100 # Start with high value to show low confidence - self.ball_position_msg: PointStamped = PointStamped() - self.pose_msg: PlayerStatusPose = PlayerStatusPose() - - # Create subscribers - self.create_subscription(PointStamped, "hsl_gamecontroller/ball_position", self.ball_position_cb, 1) - self.create_subscription(Float32, "hsl_gamecontroller/ball_age", self.ball_age_cb, 1) - self.create_subscription(Bool, "hsl_gamecontroller/is_fallen", self.is_fallen_cb, 1) - self.create_subscription(PlayerStatusPose, "hsl_gamecontroller/pose", self.pose_cb, 1) + self.player_state_msg: PlayerStateResponse = PlayerStateResponse() + self.create_subscription(PlayerStateResponse, "player_state_response", self.player_state_cb, 1) self.get_logger().info(f"We are playing as player {self.player_number} in team {self.team_number}") - # The publisher for the game state self.state_publisher = self.create_publisher(GameState, "gamestate", 1) - - # The publisher for the diagnostics self.diagnostic_pub = self.create_publisher(DiagnosticArray, "diagnostics", 1) - # The time in seconds after which we assume the game controller is lost - # and we tell the robot to move - self.game_controller_lost_time = 5 + # The time in seconds after which we assume the game controller is lost and publish a warning in the diagnostics + self.game_controller_lost_timeout: int = self.get_parameter("lost_timeout").get_parameter_value().integer_value # The address listening on and the port for sending back the robots meta data - self.addr = (self.get_parameter("listen_host").value, self.get_parameter("listen_port").value) + self.listening_address = (self.get_parameter("listen_host").value, self.get_parameter("listen_port").value) self.answer_port = self.get_parameter("answer_port").value # The time of the last package self.last_package_time: Time = self.get_clock().now() # Create the socket we want to use for the communications - self.socket = self._open_socket() - - def ball_position_cb(self, msg: PointStamped): - self.ball_position_msg = msg + self.socket = self._open_socket(self.listening_address) - def ball_age_cb(self, msg: Float32): - self.ball_age = msg.data + def player_state_cb(self, msg: PlayerStateResponse): + self.player_state_msg = msg - def is_fallen_cb(self, msg: Bool): - self.is_fallen = msg.data + def _open_socket(self, address: tuple[str, str]) -> socket.socket: + """Creates a UDP socket to listen on the given address and port with a timeout of 2 seconds.""" + udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + udp_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + udp_socket.bind(address) + udp_socket.settimeout(2) - def pose_cb(self, msg: PlayerStatusPose): - self.pose_msg = msg - - def _open_socket(self) -> socket.socket: - """Creates the socket""" - new_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) - new_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) - new_socket.bind(self.addr) - new_socket.settimeout(2) - return new_socket + return udp_socket def receive_forever(self): - """Waits in a loop for new packages""" + """Waits in a loop for new packages from the game controller.""" while rclpy.ok(): # Try to receive a package self.receive_and_answer_once() # Check if we didn't receive a package for a long time for publishing diagnostics received_message_lately = self.get_time_since_last_package() < Duration( - seconds=self.game_controller_lost_time + seconds=self.game_controller_lost_timeout ) self.publish_diagnostics(received_message_lately) def receive_and_answer_once(self): - """Receives a package, interprets it and sends an answer.""" + """ + Receives a package, parses it and publishes the GameState. + Then sends back the current player state to the game controller. + """ try: # Receive the package - data, peer = self.socket.recvfrom(GameStateStruct.sizeof()) + data, peer = self.socket.recvfrom(GameControlDataStruct.sizeof()) - # Parse the package based on the GameStateStruct + # Parse the package based on the game controllers GameControlDataStruct. # This throws a ConstError if it doesn't work - parsed_state = GameStateStruct.parse(data) - - # Assign the new package after it parsed successful to the state + parsed_state = GameControlDataStruct.parse(data) self.last_package_time = self.get_clock().now() - # Build the game state message and publish it self.state_publisher.publish(self.build_game_state_msg(parsed_state)) + self.answer_to_game_controller(peer) - # Answer the GameController - self.answer_to_gamecontroller(peer) - - except AssertionError as ae: - self.get_logger().error(str(ae)) - except socket.timeout: + except AssertionError as e: + self.get_logger().error(str(e)) + except TimeoutError: self.get_logger().info("No GameController message received (socket timeout)", throttle_duration_sec=5) - except ConstError: - self.get_logger().warn("Parse Error: Probably using an old protocol!") - except IOError as e: + except ConstError as e: + self.get_logger().error(f"Parse Error: Probably using an old protocol! {str(e)}") + except OSError as e: self.get_logger().warn(f"Error while sending keep-alive: {str(e)}") def publish_diagnostics(self, received_message_lately: bool): """ - This publishes a Diagnostics Array. + This publishes a Diagnostics Array message with the status of the game controller connection. """ - # initialize DiagnsticArray message diag_array = DiagnosticArray() - - # configure DiagnosticStatus message diag = DiagnosticStatus(name="Game Controller", hardware_id="Game Controller") + if not received_message_lately: self.get_logger().info("No GameController message received", throttle_duration_sec=5) diag.message = ( @@ -188,36 +171,58 @@ def publish_diagnostics(self, received_message_lately: bool): diag_array.header.stamp = self.get_clock().now().to_msg() self.diagnostic_pub.publish(diag_array) - def answer_to_gamecontroller(self, peer): - """Sends a life sign to the game controller""" - # Build the answer package - data = ResponseStruct.build( + def build_game_control_return_data(self, player_state: PlayerStateResponse) -> bytes: + """ + Convert a player state message into a serialized GameControlReturnData packet. + Which expects standardized units of millimeters and uses the euler yaw angle for player orientation. + """ + pose = [ + player_state.pose.pose.position.x * 1000.0, + player_state.pose.pose.position.y * 1000.0, + euler_from_quaternion(numpify(player_state.pose.pose.orientation))[2] + ] + + ball_age = -1.0 + ball_timestamp = Time.from_msg(player_state.ball.header.stamp) + if ball_timestamp.nanoseconds > 0: + ball_age = (self.get_clock().now() - ball_timestamp).nanoseconds / 1e9 + + ball = [ + player_state.ball.point.x * 1000.0, + player_state.ball.point.y * 1000.0, + ] + + return GameControlReturnDataStruct.build( dict( player_number=self.player_number, team_number=self.team_number, - fallen=self.is_fallen, - pose=self.pose_msg.pose, - ball_age=self.ball_age, - ball=[self.ball_position_msg.point.x, self.ball_position_msg.point.y], + fallen=player_state.fallen, + pose=pose, + ball_age=ball_age, + ball=ball ) ) - # Send the package + + def answer_to_game_controller(self, peer): + """Send the latest player state to the GameController.""" + data = self.build_game_control_return_data(self.player_state_msg) + self.get_logger().debug(f"Sending answer to {peer[0]}:{self.answer_port}") try: self.socket.sendto(data, (peer[0], self.answer_port)) except Exception as e: self.get_logger().error(f"Network Error: {str(e)}") - def build_game_state_msg(self, state) -> GameState: - """Builds a GameState message from the game state""" + def build_game_state_msg(self, game_control_data) -> GameState: + """Builds a GameState ros message from the game state received from the game controller""" # Get the team objects sorted into own and rival team - own_team = GameStateReceiver.select_team_by(lambda team: team.team_number == self.team_number, state.teams) - rival_team = GameStateReceiver.select_team_by(lambda team: team.team_number != self.team_number, state.teams) + own_team = GameStateReceiver.select_team_by(lambda team: team.team_number == self.team_number, game_control_data.teams) + rival_team = GameStateReceiver.select_team_by(lambda team: team.team_number != self.team_number, game_control_data.teams) # Add some assertions to make sure everything is fine assert not (own_team is None or rival_team is None), ( - f"Team {self.team_number} not playing, only {state.teams[0].team_number} and {state.teams[1].team_number}" + f"Team {self.team_number} not playing, only {game_control_data.teams[0].team_number} and {game_control_data.teams[1].team_number}" ) assert self.player_number <= len(own_team.players), f"Robot {self.player_number} not playing" @@ -226,39 +231,38 @@ def build_game_state_msg(self, state) -> GameState: return GameState( header=Header(stamp=self.get_clock().now().to_msg()), - players_per_team=state.players_per_team, - competition_type=state.competition_type.intvalue, - game_phase=state.game_phase.intvalue, - main_state=state.state.intvalue, - set_play=state.set_play.intvalue, - kicking_team=state.kicking_team, - first_half=state.first_half, - stopped=state.stopped, + players_per_team=game_control_data.players_per_team, + competition_type=game_control_data.competition_type.intvalue, + game_phase=game_control_data.game_phase.intvalue, + main_state=game_control_data.state.intvalue, + set_play=game_control_data.set_play.intvalue, + kicking_team=game_control_data.kicking_team, + first_half=game_control_data.first_half, + stopped=game_control_data.stopped, own_score=own_team.score, rival_score=rival_team.score, - secs_remaining=state.secs_remaining, - secondary_time=state.secondary_time, - penalized=this_robot.penalty != 0, + seconds_remaining=game_control_data.secs_remaining, + secondary_time=game_control_data.secondary_time, + penalized=this_robot.penalty.intvalue != GameState.PENALTY_NONE, + # In the current rules the only in place penalty is the motion in set + penalized_in_place=this_robot.penalty.intvalue == GameState.PENALTY_MOTION_IN_SET, seconds_till_unpenalized=this_robot.secs_till_unpenalized, - warings=this_robot.warnings, cautions=this_robot.cautions, + has_yellow_card=this_robot.cautions== 1, + has_red_card=this_robot.penalty.intvalue == GameState.PENALTY_SENT_OFF, own_player_color=own_team.field_player_color.intvalue, own_goalie_color=own_team.goalkeeper_color.intvalue, rival_player_color=rival_team.field_player_color.intvalue, rival_goalie_color=rival_team.goalkeeper_color.intvalue, - # --- Gibt es nicht mehr? --- - # drop_in_team = state.drop_in_team, - # drop_in_time = state.drop_in_time, penalty_shot=own_team.penalty_shot, single_shots=own_team.single_shots, - # --- Gibt es nicht mehr? Waren die ähnlich wie Message Budget? --- - # coach_message = own_team.coach_message, message_budget=own_team.message_budget, - team_mates_with_penalty=[player.penalty != 0 for player in own_team.players], - # --- Gibt es nicht mehr? Selber bemerken ob man rot hat? --- - # team_mates_with_red_card = [player.number_of_red_cards != 0 for player in own_team.players], + team_mates_with_penalty=[player.penalty.intvalue != GameState.PENALTY_NONE for player in own_team.players], + team_mates_with_yellow_card = [player.cautions == 1 for player in own_team.players], + team_mates_with_red_card = [player.penalty.intvalue == GameState.PENALTY_SENT_OFF for player in own_team.players], ) + def get_time_since_last_package(self) -> Duration: """Returns the time in seconds since the last package was received""" return self.get_clock().now() - self.last_package_time diff --git a/game_controller_hsl/game_controller_hsl/utils.py b/game_controller_hsl/game_controller_hsl/utils.py index 58a24d5..4eae9cd 100644 --- a/game_controller_hsl/game_controller_hsl/utils.py +++ b/game_controller_hsl/game_controller_hsl/utils.py @@ -13,13 +13,14 @@ def get_parameters_from_other_node(own_node: Node, """ Used to receive parameters from other running nodes. Returns a dict with requested parameter name as dict key and parameter value as dict value. - + From bitbots_utils (https://github.com/bit-bots/bitbots_misc) """ client = own_node.create_client(GetParameters, f'{other_node_name}/get_parameters') ready = client.wait_for_service(timeout_sec=service_timeout_sec) if not ready: - raise RuntimeError(f'Wait for {other_node_name} parameter service timed out') + raise TimeoutError(f'Wait for {other_node_name} parameter service timed out') + request = GetParameters.Request() request.names = parameter_names future = client.call_async(request) @@ -29,4 +30,5 @@ def get_parameters_from_other_node(own_node: Node, results = {} # Received parameter for i, param in enumerate(parameter_names): results[param] = parameter_value_to_python(response.values[i]) - return results \ No newline at end of file + + return results diff --git a/game_controller_hsl/launch/game_controller.launch b/game_controller_hsl/launch/game_controller.launch index 1e4cedb..4c03143 100644 --- a/game_controller_hsl/launch/game_controller.launch +++ b/game_controller_hsl/launch/game_controller.launch @@ -28,7 +28,7 @@ - + diff --git a/game_controller_hsl/package.xml b/game_controller_hsl/package.xml index e86607b..c2916b5 100644 --- a/game_controller_hsl/package.xml +++ b/game_controller_hsl/package.xml @@ -5,11 +5,10 @@ 1.1.0 The game_controller_hsl packages receives packets from the GameController and - republishes them as GameState ROS messages. It sends response packets - back to the GameController. + republishes them as GameState ROS messages. It sends response packets based on the + PlayerStateResponse ROS messages back to the GameController. - Timon Engelke Hamburg Bit-Bots Apache License 2.0 @@ -19,11 +18,14 @@ rosidl_default_generators rosidl_default_runtime - diagnostic_msgs game_controller_hsl_interfaces + tf_transformations python3-construct + ros2-numpy rclpy std_msgs + geometry_msgs + diagnostic_msgs ament_python diff --git a/game_controller_hsl/scripts/sim_gamestate.py b/game_controller_hsl/scripts/sim_gamestate.py index 0b46c1d..b892071 100755 --- a/game_controller_hsl/scripts/sim_gamestate.py +++ b/game_controller_hsl/scripts/sim_gamestate.py @@ -5,182 +5,258 @@ # The script provides a simple mechanism to test robot behavior in different game states, # when no game controller is running -import sys import select +import sys import termios import tty import rclpy +from game_controller_hsl.utils import get_parameters_from_other_node +from game_controller_hsl_interfaces.msg import GameState from rclpy.node import Node -from rclpy.qos import QoSProfile, DurabilityPolicy +from rclpy.qos import DurabilityPolicy, QoSProfile + + +class SimGameState(Node): + DEFAULT_PLAYERS_PER_TEAM = 4 + STATE_KEYS = { + "0": ("STATE_INITIAL", GameState.STATE_INITIAL), + "1": ("STATE_READY", GameState.STATE_READY), + "2": ("STATE_SET", GameState.STATE_SET), + "3": ("STATE_PLAYING", GameState.STATE_PLAYING), + "4": ("STATE_FINISHED", GameState.STATE_FINISHED), + } + COMPETITION_TYPE_KEYS = { + "5": ("COMPETITION_TYPE_SMALL", GameState.COMPETITION_TYPE_SMALL), + "6": ("COMPETITION_TYPE_MIDDLE", GameState.COMPETITION_TYPE_MIDDLE), + "7": ("COMPETITION_TYPE_LARGE", GameState.COMPETITION_TYPE_LARGE), + } + GAME_PHASE_KEYS = { + "a": ("GAME_PHASE_NORMAL", GameState.GAME_PHASE_NORMAL), + "b": ("GAME_PHASE_PENALTY_SHOOT_OUT", GameState.GAME_PHASE_PENALTY_SHOOT_OUT), + "c": ("GAME_PHASE_EXTRA_TIME", GameState.GAME_PHASE_EXTRA_TIME), + "d": ("GAME_PHASE_TIMEOUT", GameState.GAME_PHASE_TIMEOUT), + } + SET_PLAY_KEYS = { + "e": ("SET_PLAY_NONE", GameState.SET_PLAY_NONE), + "f": ("SET_PLAY_DIRECT_FREE_KICK", GameState.SET_PLAY_DIRECT_FREE_KICK), + "g": ("SET_PLAY_INDIRECT_FREE_KICK", GameState.SET_PLAY_INDIRECT_FREE_KICK), + "h": ("SET_PLAY_PENALTY_KICK", GameState.SET_PLAY_PENALTY_KICK), + "i": ("SET_PLAY_THROW_IN", GameState.SET_PLAY_THROW_IN), + "j": ("SET_PLAY_GOAL_KICK", GameState.SET_PLAY_GOAL_KICK), + "k": ("SET_PLAY_CORNER_KICK", GameState.SET_PLAY_CORNER_KICK), + } -from game_controller_hsl_interfaces.msg import GameState -from game_controller_hsl.utils import get_parameters_from_other_node + def __init__(self): + super().__init__("sim_gamestate") + self.logger = self.get_logger() + params = self.load_parameters() + self.team_id = int(params["team_id"]) + self.player_number = int(params["bot_id"]) + self.has_kick = True -class SimGamestate(Node): - msg = """Setting the GameState by entering a number: + self.settings = termios.tcgetattr(sys.stdin) -0: STATE_INITIAL = 0 -1: STATE_READY = 1 -2: STATE_SET = 2 -3: STATE_PLAYING =3 -4: STATE_FINISHED = 4 + self.publisher = self.create_publisher( + GameState, + "gamestate", + QoSProfile(durability=DurabilityPolicy.TRANSIENT_LOCAL, depth=1), + ) -5: COMPETITION_TYPE_SMALL = 0 -6: COMPETITION_TYPE_MIDDLE = 1 -7: COMPETITION_TYPE_LARGE = 3 + def load_parameters(self) -> dict[str, int | str]: + """ + Tries fetching team_id and bot_id from parameter blackboard. + If the blackboard is unavailable, fall back to manual/default values. + """ + try: + self.logger.info("Trying to fetch team_id and bot_id from parameter blackboard") + params = get_parameters_from_other_node( + self, + "parameter_blackboard", + ["team_id", "bot_id"], + service_timeout_sec=5.0, + ) + except TimeoutError: + self.logger.warn("Parameter blackboard not available") + params = {} + + team_id = params.get("team_id") + bot_id = params.get("bot_id") + + if team_id is None: + self.logger.warn("No team_id found in parameter blackboard") + params["team_id"]= int(input("Please enter team id: ")) + + if bot_id is None: + self.logger.warn("No bot_id found in parameter blackboard, assuming player 1") + params["bot_id"] = 1 + + return params + + def create_game_state_msg(self): + game_state_msg = GameState() + game_state_msg.header.stamp = self.get_clock().now().to_msg() + game_state_msg.players_per_team = self.DEFAULT_PLAYERS_PER_TEAM + game_state_msg.kicking_team = self.team_id + + self.sync_team_mates(game_state_msg) + return game_state_msg + + def apply_key(self, game_state_msg, key): + if key == "\x03": + return False + elif key in self.STATE_KEYS: + game_state_msg.main_state = self.STATE_KEYS[key][1] + elif key in self.COMPETITION_TYPE_KEYS: + game_state_msg.competition_type = self.COMPETITION_TYPE_KEYS[key][1] + elif key in self.GAME_PHASE_KEYS: + game_state_msg.game_phase = self.GAME_PHASE_KEYS[key][1] + elif key in self.SET_PLAY_KEYS: + game_state_msg.set_play = self.SET_PLAY_KEYS[key][1] + elif key == "p": + game_state_msg.penalized = not game_state_msg.penalized + + if game_state_msg.penalized: + # Use any generic penalty type, which is not in place as they + # all lead to the same timeout penalty and are likely handled + # the same way + game_state_msg.penalty = GameState.PENALTY_ILLEGAL_POSITIONING + else: + # If penalized is toggled off, we also cannot be penalized in place anymore + game_state_msg.penalized_in_place = False + elif key == "l": + game_state_msg.penalized_in_place = not game_state_msg.penalized_in_place + game_state_msg.penalized = game_state_msg.penalized_in_place + game_state_msg.penalty = ( + GameState.PENALTY_MOTION_IN_SET if game_state_msg.penalized_in_place else GameState.PENALTY_NONE + ) + elif key == "y": + game_state_msg.has_yellow_card = not game_state_msg.has_yellow_card + if game_state_msg.has_yellow_card: + game_state_msg.has_red_card = False + game_state_msg.cautions = 1 + else: + game_state_msg.cautions = 0 + elif key == "r": + game_state_msg.has_red_card = not game_state_msg.has_red_card + if game_state_msg.has_red_card: + game_state_msg.has_yellow_card = False + game_state_msg.cautions = 2 + game_state_msg.penalized = True + game_state_msg.penalized_in_place = False + game_state_msg.penalty = GameState.PENALTY_SENT_OFF + else: + game_state_msg.cautions = 0 + game_state_msg.penalized = False + game_state_msg.penalty = GameState.PENALTY_NONE + elif key == "t": + if game_state_msg.kicking_team == self.team_id: + game_state_msg.kicking_team = self.team_id + 1 + else: + game_state_msg.kicking_team = self.team_id + elif key == "s": + game_state_msg.stopped = not game_state_msg.stopped + elif key == "+": + game_state_msg.own_score += 1 + game_state_msg.main_state = GameState.STATE_READY + elif key == "-": + game_state_msg.rival_score += 1 + game_state_msg.main_state = GameState.STATE_READY + + self.sync_team_mates(game_state_msg) + return True + + def sync_team_mates(self, game_state_msg): + player_count = max(game_state_msg.players_per_team, self.player_number) + player_index = self.player_number - 1 + + game_state_msg.team_mates_with_penalty = [False] * player_count + game_state_msg.team_mates_with_yellow_card = [False] * player_count + game_state_msg.team_mates_with_red_card = [False] * player_count + + game_state_msg.team_mates_with_penalty[player_index] = game_state_msg.penalized + game_state_msg.team_mates_with_yellow_card[player_index] = game_state_msg.has_yellow_card + + def key_mapping_info(self ) -> str: + def build_key_mapping_string(key_mapping: dict[str, tuple[str, int]]) -> str: + bindings = [] + for key, mapping in key_mapping.items(): + bindings.append(f"{key}: {mapping[0]} = {mapping[1]}") + + return "\n".join(bindings) + + return f""" +{build_key_mapping_string(self.STATE_KEYS)} + +{build_key_mapping_string(self.COMPETITION_TYPE_KEYS)} Set the game phase by entering: -a: GAME_PHASE_TIMEOUT = 0 -b: GAME_PHASE_NORMAL = 1 -c: GAME_PHASE_EXTRA_TIME = 2 -d: GAME_PHASE_PENALTY_SHOOT_OUT = 3 +{build_key_mapping_string(self.GAME_PHASE_KEYS)} Set play states by entering: -e: SET_PLAY_NONE = 0 -f: SET_PLAY_DIRECT_FREE_KICK = 1 -g: SET_PLAY_INDIRECT_FREE_KICK = 2 -h: SET_PLAY_PENALTY_KICK = 3 -i: SET_PLAY_THROW_IN = 4 -j: SET_PLAY_GOAL_KICK = 5 -k: SET_PLAY_CORNER_KICK = 6 +{build_key_mapping_string(self.SET_PLAY_KEYS)} p: toggle penalized +l: toggle in place penalty +y: toggle yellow card +r: toggle red card t: toggle kicking team s: toggle stopped state +: increase own score by 1 -: increase rival score by 1 +Competition Type: 0, Game Phase: 0 +Main State: 0, Set Play: 0 +Kicking Team: +Penalized: +In Place Penalized: +Yellow Card: +Red Card: +Stopped: +Goals(Own : Rival): +CTRL-C to quit""" + def game_state_info(self, game_state_msg: GameState): + return f""" +Competition Type: {game_state_msg.competition_type}, Game Phase: {game_state_msg.game_phase} +Main State: {game_state_msg.main_state}, Set Play: {game_state_msg.set_play} +Kicking Team: {game_state_msg.kicking_team} +Penalized: {game_state_msg.penalized} +In Place Penalized: {game_state_msg.penalized_in_place} +Yellow Card: {game_state_msg.has_yellow_card} +Red Card: {game_state_msg.has_red_card} +Stopped: {game_state_msg.stopped} +Goals(Own : Rival): {game_state_msg.own_score} : {game_state_msg.rival_score} - - - - - - - - - - - - - - - - - - - -CTRL-C to quit -""" - - def __init__(self): - super().__init__("sim_gamestate") - self.logger = self.get_logger() - - # Try fetching team id from parameter blackboard or ask user for input - try: - self.team_id = get_parameters_from_other_node(self, "parameter_blackboard", ["team_id"])["team_id"] - except KeyError: - self.logger.error("No team id found in parameter blackboard") - self.team_id = int(input("Please enter team id: ")) - - self.has_kick_off = True - - self.settings = termios.tcgetattr(sys.stdin) - - self.publisher = self.create_publisher( - GameState, - "gamestate", - QoSProfile(durability=DurabilityPolicy.TRANSIENT_LOCAL, depth=1), - ) +CTRL-C to quit""" def loop(self): - game_state_msg = GameState() - game_state_msg.header.stamp = self.get_clock().now().to_msg() - game_state_msg.players_per_team = 4 - # Init kicking team to our teamID - game_state_msg.kicking_team = self.team_id + game_state_msg = self.create_game_state_msg() try: - print(self.msg) + print(self.key_mapping_info()) + while True: key = self.get_key() - if key == "\x03": + if not self.apply_key(game_state_msg, key): break - elif key in ["0", "1", "2", "3", "4"]: - int_key = int(key) - game_state_msg.main_state = int_key - elif key in ["5", "6", "7"]: - int_key = int(key) - game_state_msg.competition_type = int_key - 5 - elif key == "p": # penalize / unpenalize - game_state_msg.penalized = not game_state_msg.penalized - elif key in [chr(ord("a") + x) for x in range(4)]: - game_state_msg.game_phase = ord(key) - ord("a") - elif key in [chr(ord("e") + x) for x in range(7)]: - game_state_msg.set_play = ord(key) - ord("e") - elif key == "t": - if game_state_msg.kicking_team == self.team_id: - game_state_msg.kicking_team = self.team_id + 1 - else: - game_state_msg.kicking_team = self.team_id - elif key == "s": - game_state_msg.stopped = not game_state_msg.stopped - elif key == "+": - game_state_msg.own_score += 1 - elif key == "-": - game_state_msg.rival_score += 1 - - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - sys.stdout.write("\x1b[A") - self.publisher.publish(game_state_msg) - print( - f""" - -Competition Type: {game_state_msg.competition_type} -Game Phase: {game_state_msg.game_phase} -Set Play: {game_state_msg.set_play} -Main State: {game_state_msg.main_state} - -Kicking Team: {game_state_msg.kicking_team} -Penalized: {game_state_msg.penalized} -Stopped: {game_state_msg.stopped} - -Goals(Own : Rival): {game_state_msg.own_score} : {game_state_msg.rival_score} - -CTRL-C to quit -""" - ) + game_state_str = self.game_state_info(game_state_msg) + # Override the lines: Each \n in the str + the extra \n from print + line_count = game_state_str.count("\n") + 1 + sys.stdout.write("\x1b[A\x1b[K" * line_count) + print(game_state_str) + self.publisher.publish(game_state_msg) except Exception as e: print(e) - finally: - print() - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) def get_key(self): @@ -193,7 +269,7 @@ def get_key(self): if __name__ == "__main__": rclpy.init(args=None) - node = SimGamestate() + node = SimGameState() node.loop() node.destroy_node() rclpy.shutdown() diff --git a/game_controller_hsl/test/__init__.py b/game_controller_hsl/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/game_controller_hsl/test/test_game_controller.py b/game_controller_hsl/test/test_game_controller.py deleted file mode 100644 index c26bf9f..0000000 --- a/game_controller_hsl/test/test_game_controller.py +++ /dev/null @@ -1,192 +0,0 @@ - -import rclpy - -from construct import Container, ListContainer, EnumIntegerString - -from rclpy.node import Node -from rclpy.parameter import Parameter - -from game_controller_hsl.gamestate import GameStateStruct, TeamInfoStruct -from game_controller_hsl.utils import get_parameters_from_other_node -from game_controller_hsl.receiver import GameStateReceiver - -from game_controller_hsl_interfaces.msg import GameState - - -def test_get_parameters_from_other_node(): - rclpy.init() - class MockNode(Node): - def __init__(self): - super().__init__('mock_node') - self.declare_parameter('team_id', 1) - self.declare_parameter('bot_id', 2) - - node = MockNode() - params = get_parameters_from_other_node(node, 'mock_node', ['team_id', 'bot_id']) - assert params['team_id'] == 1 - assert params['bot_id'] == 2 - node.destroy_node() - rclpy.shutdown() - - -def test_game_state_receiver_select_team_by(): - player = dict( - penalty=0, - secs_till_unpenalized=0, - ) - - # Create a TeamInfoStruct as if it was a parsed game state - team = TeamInfoStruct.build(dict( - number=1, - field_player_color=0, - goalkeeper_color=2, - goalkeeper=0, - score=3, - penalty_shot=0, - single_shots=0, - message_budget=100, - players=[player]*11 - )) - - # Parse the TeamInfoStruct and check if the selector works - teams = [TeamInfoStruct.parse(team)] - - assert GameStateReceiver.select_team_by(lambda team: team.team_number == 1, teams).team_number == 1 - assert GameStateReceiver.select_team_by(lambda team: team.team_number == 2, teams) is None - - -'''def test_parse_gamestate(): - # Create dummy game state - num_players = 11 - dummy_state_dict = dict( - header=b'RGme', - version=12, - packet_number=0, - players_per_team=num_players, - competition_phase=0, - competition_type=0, - game_phase=0, - set_play=0, - first_half=False, - kicking_team=0, - secs_remaining=0, - secondary_time=0, - teams=[dict( - number=team_id, - field_player_color=0, - goalkeeper_color=0, - goalkeeper=0, - score=0, - penalty_shot=0, - single_shots=0, - message_budget=0, - players=[dict( - penalty=0, - secs_till_unpenalized=0, - ) for player_id in range(num_players)] - ) for team_id in range(2)] - ) - - # Create a GameStateStruct as if it was a parsed game state - state = GameStateStruct.build(dummy_state_dict) - - # Binary representation of the GameStateStruct - #nullen zählen? - dummy_package = b'RGme\x0c\x00\x00\x0b\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00' - assert dummy_package == state - - # Parse the GameStateStruct and check if the values are correct - state = GameStateStruct.parse(state) - - # Recursivly convert Container to dict - def dictify(container): - new_dict = dict() - for key, value in container.items(): - if isinstance(value, Container): - new_dict[key] = dictify(value) - elif isinstance(value, ListContainer): - new_dict[key] = [dictify(item) for item in value] - elif isinstance(value, EnumIntegerString): - new_dict[key] = int(value) - elif key != '_io': - new_dict[key] = value - return new_dict - - # Check if the dict and the parsed game state are equal - assert dictify(state) == dummy_state_dict - -''' -def test_on_new_gamestate(): - # Create dummy game state - num_players = 11 - dummy_state_dict = dict( - header=b'RGme', - version=12, - packet_number=0, - players_per_team=num_players, - competition_phase=0, - competition_type=0, - game_phase=0, - set_play=0, - first_half=False, - kicking_team=0, - secs_remaining=0, - secondary_time=0, - teams=[dict( - number=team_id, - field_player_color=0, - goalkeeper_color=0, - goalkeeper=0, - score=0, - penalty_shot=0, - single_shots=0, - message_budget=0, - players=[dict( - penalty=0, - secs_till_unpenalized=0, - ) for player_id in range(num_players)] - ) for team_id in range(2)] - ) - - # Create a GameStateStruct as if it was a parsed game state - state = GameStateStruct.parse(GameStateStruct.build(dummy_state_dict)) - - # Create the game state receiver with the team and player number parameter overwritten - rclpy.init() - receiver = GameStateReceiver( - parameter_overrides=[ - Parameter('team_id', Parameter.Type.INTEGER, 1), - Parameter('bot_id', Parameter.Type.INTEGER, 1), - Parameter('listen_port', Parameter.Type.INTEGER, 3838), - Parameter('answer_port', Parameter.Type.INTEGER, 3939), - Parameter('listen_host', Parameter.Type.STRING, '0.0.0.0') - ] - ) - - # Call the on_new_gamestate method and listen to the published game state message - msg = receiver.build_game_state_msg(state) - - # Check if the message is correct - assert msg.competition_phase == 0 - assert msg.game_phase == 0 - assert msg.main_state == 0 - assert msg.set_play == 0 - assert msg.kicking_team == 0 - assert msg.first_half == False - assert msg.own_score == 0 - assert msg.rival_score == 0 - assert msg.secs_remaining == 0 - assert msg.secondary_time == 0 - assert msg.has_kick_off == False - assert msg.penalized == False - assert msg.seconds_till_unpenalized == 0 - assert msg.own_player_color == 0 - assert msg.own_goalie_color == 0 - assert msg.rival_player_color == 0 - assert msg.rival_goalie_color == 0 - assert msg.penalty_shot == 0 - assert msg.single_shots == 0 - assert msg.message_budget == 0 - assert msg.team_mates_with_penalty == 0 - - diff --git a/game_controller_hsl/test/test_receiver.py b/game_controller_hsl/test/test_receiver.py new file mode 100644 index 0000000..7b63afd --- /dev/null +++ b/game_controller_hsl/test/test_receiver.py @@ -0,0 +1,161 @@ +import math +from unittest.mock import Mock + +import pytest +from builtin_interfaces.msg import Time as TimeMsg +from game_controller_hsl.data import GameControlDataStruct, GameControlReturnDataStruct +from game_controller_hsl.receiver import GameStateReceiver +from game_controller_hsl_interfaces.msg import GameState, PlayerStateResponse +from geometry_msgs.msg import Point, PointStamped, Pose, PoseStamped, Quaternion +from rclpy.clock_type import ClockType +from rclpy.time import Time +from std_msgs.msg import Header + +def test_build_game_control_return_data_converts_ros_msg_to_protocol(receiver) -> None: + receiver._clock.now.return_value = Time(seconds=12, clock_type=ClockType.ROS_TIME) + + player_pose_yaw = 0.5 + response = player_state_response(player_pose_yaw) + + packet = GameControlReturnDataStruct.parse(receiver.build_game_control_return_data(response)) + + assert packet.player_number == 2 + assert packet.team_number == 6 + assert packet.fallen is True + assert packet.pose == pytest.approx([1000.0, -2000.0, player_pose_yaw]) + assert packet.ball_age == pytest.approx(2.0) + assert packet.ball == pytest.approx([300.0, -400.0]) + + +def test_build_game_control_return_data_marks_unseen_ball(receiver) -> None: + packet = GameControlReturnDataStruct.parse( + receiver.build_game_control_return_data(PlayerStateResponse()) + ) + + assert packet.ball_age == -1.0 + assert packet.pose == pytest.approx([0.0, 0.0, 0.0]) + assert packet.ball == pytest.approx([0.0, 0.0]) + assert packet.fallen is False + +def test_build_game_state_msg_converts_protocol_to_ros_msg(receiver) -> None: + packet = GameControlDataStruct.parse(game_control_data()) + + msg = receiver.build_game_state_msg(packet) + + assert msg.players_per_team == 4 + assert msg.competition_type == 0 + assert msg.game_phase == GameState.GAME_PHASE_NORMAL + assert msg.main_state == GameState.STATE_PLAYING + assert msg.set_play == GameState.SET_PLAY_NONE + assert msg.kicking_team == 1 + assert msg.first_half is True + assert msg.stopped is False + assert msg.own_score == 1 + assert msg.rival_score == 0 + assert msg.seconds_remaining == 300 + assert msg.secondary_time == 0 + assert msg.penalized is True + assert msg.penalized_in_place is True + assert msg.seconds_till_unpenalized == 16 + assert msg.cautions == 1 + assert msg.has_yellow_card is True + assert msg.has_red_card is False + assert msg.own_player_color == GameState.TEAM_RED + assert msg.own_goalie_color == GameState.TEAM_BLACK + assert msg.rival_player_color == GameState.TEAM_BLUE + assert msg.rival_goalie_color == GameState.TEAM_WHITE + assert msg.penalty_shot == 0 + assert msg.single_shots == 0b0000000000000000 + assert msg.message_budget == 10 + assert msg.team_mates_with_penalty == [True, True] + [False] * 18 + assert msg.team_mates_with_yellow_card == [False, True] + [False] * 18 + assert msg.team_mates_with_red_card == [True, False] + [False] * 18 + +def game_control_data() -> bytes: + return GameControlDataStruct.build( + dict( + packet_number=1, + players_per_team=4, + competition_type=0, # COMPETITION_TYPE_SMALL + stopped=False, + game_phase=0, # GAME_PHASE_NORMAL + state=3, # STATE_PLAYING + set_play=0, # SET_PLAY_NONE + first_half=True, + kicking_team=1, + secs_remaining=300, + secondary_time=0, + teams=[ + dict( + team_number=6, + field_player_color=1, # "TEAM_COLOR_RED + goalkeeper_color=3, # "TEAM_COLOR_BLACK + goalkeeper=1, + score=1, + penalty_shot=0, + single_shots=0b0000000000000000, + message_budget=10, + players=[ + dict( + penalty=12, # PENALTY_SENT_OFF + secs_till_unpenalized=0, + cautions=2, + ), + dict( + penalty=2, # PENALTY_MOTION_IN_SET + secs_till_unpenalized=16, + cautions=1, + ), + ] + [ + dict( + penalty=0, # PENALTY_NONE + secs_till_unpenalized=0, + cautions=0, + ) for _ in range(18) + ] + ), + dict( + team_number=7, + field_player_color=0, # TEAM_COLOR_BLUE + goalkeeper_color=4, # TEAM_COLOR_WHITE + goalkeeper=1, + score=0, + penalty_shot=0, + single_shots=0b0000000000000000, + message_budget=10, + players=[ + dict( + penalty=0, # PENALTY_NONE + secs_till_unpenalized=0, + cautions=0, + ) for _ in range(20) + ], + ), + ], + ) + ) + +def player_state_response(player_pose_yaw) -> PlayerStateResponse: + return PlayerStateResponse( + fallen=True, + pose=PoseStamped( + pose=Pose( + position=Point(x=1.0, y=-2.0, z=0.0), + orientation=Quaternion(z=math.sin(player_pose_yaw / 2.0), w=math.cos(player_pose_yaw / 2.0)), + ) + ), + ball=PointStamped( + header=Header(stamp=TimeMsg(sec=10)), + point=Point(x=0.3, y=-0.4), + ), + ) + + +@pytest.fixture +def receiver() -> GameStateReceiver: + receiver = object.__new__(GameStateReceiver) + receiver.player_number = 2 + receiver.team_number = 6 + receiver._clock = Mock() + + return receiver diff --git a/game_controller_hsl_interfaces/CMakeLists.txt b/game_controller_hsl_interfaces/CMakeLists.txt index 0f527ed..100166c 100644 --- a/game_controller_hsl_interfaces/CMakeLists.txt +++ b/game_controller_hsl_interfaces/CMakeLists.txt @@ -14,7 +14,7 @@ endif() rosidl_generate_interfaces(${PROJECT_NAME} "msg/GameState.msg" - "msg/PlayerStatusPose.msg" + "msg/PlayerStateResponse.msg" DEPENDENCIES std_msgs geometry_msgs diff --git a/game_controller_hsl_interfaces/msg/GameState.msg b/game_controller_hsl_interfaces/msg/GameState.msg index a98eb00..8904b78 100644 --- a/game_controller_hsl_interfaces/msg/GameState.msg +++ b/game_controller_hsl_interfaces/msg/GameState.msg @@ -1,10 +1,16 @@ -# This message provides all information from the game controller -# for additional information see documentation of the game controller -# https://github.com/bhuman/GameController - +# This message defines a GameState extracted from the RoboCupGameControlData packet received by the game controller. +# While the game controller provides info about both teams and all players this message represents the +# state for the currently running team/player combination. +# +# For the packet defition for RoboCupGameControlData see: +# https://github.com/RoboCup-HumanoidSoccerLeague/GameController/blob/master/game_controller_msgs/headers/RoboCupGameControlData.h +# +# For additional documentation of the underlying meaning of fields have a look at: +# https://github.com/RoboCup-HumanoidSoccerLeague/HSL-Rules std_msgs/Header header +##### Game State uint8 players_per_team uint8 COMPETITION_TYPE_SMALL = 0 @@ -34,20 +40,18 @@ uint8 SET_PLAY_GOAL_KICK = 5 uint8 SET_PLAY_CORNER_KICK = 6 uint8 set_play -uint8 kicking_team bool first_half bool stopped +uint8 kicking_team uint8 own_score uint8 rival_score # Seconds remaining for the game half -int16 secs_remaining -# Seconds remaining for things like kickoff +int16 seconds_remaining +# Seconds remaining for current game action (e.g. kickoff) int16 secondary_time -bool penalized -uint16 seconds_till_unpenalized -uint8 warnings -uint8 cautions + +##### TeamInfo uint8 TEAM_BLUE = 0 uint8 TEAM_RED = 1 @@ -69,5 +73,41 @@ uint8 penalty_shot # a binary pattern indicating the successful penalty shots (1 for successful, 0 for unsuccessful) uint16 single_shots -uint8 message_budget +# Number of remaining messages, that can be sent by the team +uint16 message_budget + +# Extra fields to make decisions based on number of active or cautioned players. +# The indexes represent the players in order. bool[] team_mates_with_penalty +bool[] team_mates_with_yellow_card +bool[] team_mates_with_red_card + +##### RobotInfo + +uint8 PENALTY_NONE = 0 +uint8 PENALTY_ILLEGAL_POSITIONING = 1 +uint8 PENALTY_MOTION_IN_SET = 2 +uint8 PENALTY_MOTION_IN_STOP = 3 +uint8 PENALTY_LOCAL_GAME_STUCK = 4 +uint8 PENALTY_INCAPABLE_ROBOT = 5 +uint8 PENALTY_PICK_UP = 6 +uint8 PENALTY_BALL_HOLDING = 7 +uint8 PENALTY_LEAVING_THE_FIELD = 8 +uint8 PENALTY_PLAYING_WITH_ARMS_HANDS = 9 +uint8 PENALTY_PUSHING = 10 +uint8 PENALTY_CAUTIONED = 11 +uint8 PENALTY_SENT_OFF = 12 +uint8 PENALTY_SUBSTITUTE = 13 +uint8 penalty + +bool penalized +# Extra fields to differentiate between penalties, which will have the robot +# removed from the game to the sideline and penalties that happen in place +# on the field (e.g. MOTION_IN_SET) +bool penalized_in_place +uint8 seconds_till_unpenalized + +# Number of yellow cards the player has received +uint8 cautions +bool has_yellow_card +bool has_red_card diff --git a/game_controller_hsl_interfaces/msg/PlayerStateResponse.msg b/game_controller_hsl_interfaces/msg/PlayerStateResponse.msg new file mode 100644 index 0000000..e914ea6 --- /dev/null +++ b/game_controller_hsl_interfaces/msg/PlayerStateResponse.msg @@ -0,0 +1,39 @@ +# This message defines a PlayerStateResponse to be converted and sent as a RoboCupGameControlReturnData packet to the game controller. +# It is used to verify a player is connected to game controller and for visualization purposes. +# +# For the packet defition for RoboCupGameControlReturnData see: +# https://github.com/RoboCup-HumanoidSoccerLeague/GameController/blob/master/game_controller_msgs/headers/RoboCupGameControlData.h + +# Time at which this response snapshot was assembled. frame_id is unused; +# pose and ball carry their own coordinate frames. +std_msgs/Header header + +bool fallen + +# This pose is based on the following field coordinate system: +# +# y +# ^ ______________________ +# | M | | | O +# | Y |_ -x, y | x, y _| P +# | G | | | | | P +# 0 + O | | ( ) | | G +# | A |_| | |_| O +# | L | -x,-y | x,-y | A +# | |__________|__________| L +# | +# +------------------+--------------> x +# 0 +# +# 0,0 is the center of the field. pose.header.stamp is the time of the pose +# estimate and pose.header.frame_id identifies the field coordinate frame. +# Position is expressed in metres. The orientation represents yaw about the +# +z axis; it is converted to an angle in radians for the return packet, with +# yaw 0 along the +x axis and increasing counter-clockwise. +geometry_msgs/PoseStamped pose + +# Ball position in metres, relative to the robot. ball.header.stamp is the time +# the ball was last observed and ball.header.frame_id identifies the applicable +# robot-relative coordinate frame, whose +x axis points forward and +y axis +# points left. +geometry_msgs/PointStamped ball diff --git a/game_controller_hsl_interfaces/msg/PlayerStatusPose.msg b/game_controller_hsl_interfaces/msg/PlayerStatusPose.msg deleted file mode 100644 index f2f6f21..0000000 --- a/game_controller_hsl_interfaces/msg/PlayerStatusPose.msg +++ /dev/null @@ -1,22 +0,0 @@ -# This Pose message is based on the following field coordinate system: -# -# y -# ^ ______________________ -# | M | | | O -# | Y |_ -x, y | x, y _| P -# | G | | | | | P -# 0 + O | | ( ) | | G -# | A |_| | |_| O -# | L | -x,-y | x,-y | A -# | |__________|__________| L -# | -# +------------------+--------------> x -# 0 -# -# 0,0 is the center of the filed and we expect (x, y, theta) with theta -# being the euler angle in radians, 0 along the +x axis, -# increasing counter clockwise - -std_msgs/Header header - -float32[3] pose diff --git a/game_controller_hsl_interfaces/package.xml b/game_controller_hsl_interfaces/package.xml index 07ee19e..f0d673f 100644 --- a/game_controller_hsl_interfaces/package.xml +++ b/game_controller_hsl_interfaces/package.xml @@ -5,7 +5,6 @@ 0.0.1 RoboCup Humanoid League Game Controller Messages - Florian Vahl Hamburg Bit-Bots Apache License 2.0 @@ -15,8 +14,9 @@ ament_cmake rosidl_default_generators - std_msgs ament_lint_auto + geometry_msgs + std_msgs rosidl_default_generators rosidl_default_runtime