From b6547f7534f2787d886b943184ee34142ea41f4f Mon Sep 17 00:00:00 2001 From: Florian Vahl <7vahl@informatik.uni-hamburg.de> Date: Tue, 16 Jan 2024 15:40:04 +0100 Subject: [PATCH] Remove bitbots deps, cleanup and tests --- .../game_controller_humanoid/gamestate.py | 16 +- .../game_controller_humanoid/receiver.py | 297 +++++++++--------- .../game_controller_humanoid/utils.py | 32 ++ game_controller_humanoid/package.xml | 9 +- game_controller_humanoid/setup.py | 4 +- .../test/test_game_controller.py | 220 +++++++++++++ .../.gitignore | 12 + .../CMakeLists.txt | 19 ++ .../msg/GameState.msg | 67 ++++ .../package.xml | 27 ++ 10 files changed, 536 insertions(+), 167 deletions(-) create mode 100644 game_controller_humanoid/game_controller_humanoid/utils.py create mode 100644 game_controller_humanoid/test/test_game_controller.py create mode 100644 game_controller_humanoid_interfaces/.gitignore create mode 100644 game_controller_humanoid_interfaces/CMakeLists.txt create mode 100644 game_controller_humanoid_interfaces/msg/GameState.msg create mode 100644 game_controller_humanoid_interfaces/package.xml diff --git a/game_controller_humanoid/game_controller_humanoid/gamestate.py b/game_controller_humanoid/game_controller_humanoid/gamestate.py index b695e6e..1ec856a 100644 --- a/game_controller_humanoid/game_controller_humanoid/gamestate.py +++ b/game_controller_humanoid/game_controller_humanoid/gamestate.py @@ -5,7 +5,7 @@ Short = Int16ul -RobotInfo = "robot_info" / Struct( +RobotInfoStruct = "robot_info" / Struct( # define NONE 0 # define PENALTY_HL_KID_BALL_MANIPULATION 1 # define PENALTY_HL_KID_PHYSICAL_CONTACT 2 @@ -23,7 +23,7 @@ "goalkeeper" / Flag ) -TeamInfo = "team" / Struct( +TeamInfoStruct = "team" / Struct( "team_number" / Byte, "team_color" / Enum(Byte, BLUE=0, @@ -42,11 +42,11 @@ "single_shots" / Short, # bits represent penalty shot success "coach_sequence" / Byte, "coach_message" / PaddedString(253, 'utf8'), - "coach" / RobotInfo, - "players" / Array(11, RobotInfo) + "coach" / RobotInfoStruct, + "players" / Array(11, RobotInfoStruct) ) -GameState = "gamedata" / Struct( +GameStateStruct = "gamedata" / Struct( "header" / Const(b'RGme'), "version" / Const(12, Short), "packet_number" / Byte, @@ -84,14 +84,14 @@ "drop_in_time" / Short, "seconds_remaining" / Int16sl, "secondary_seconds_remaining" / Int16sl, - "teams" / Array(2, "team" / TeamInfo) + "teams" / Array(2, "team" / TeamInfoStruct) ) GAME_CONTROLLER_RESPONSE_VERSION = 2 -ReturnData = Struct( +ResponseStruct = Struct( "header" / Const(b"RGrt"), - "version" / Const(2, Byte), + "version" / Const(GAME_CONTROLLER_RESPONSE_VERSION, Byte), "team" / Byte, "player" / Byte, "message" / Byte diff --git a/game_controller_humanoid/game_controller_humanoid/receiver.py b/game_controller_humanoid/game_controller_humanoid/receiver.py index e90a8bb..b702575 100755 --- a/game_controller_humanoid/game_controller_humanoid/receiver.py +++ b/game_controller_humanoid/game_controller_humanoid/receiver.py @@ -12,20 +12,20 @@ # See the License for the specific language governing permissions and # limitations under the License. - import socket -import time +from typing import Any import rclpy -from construct import Container, ConstError -from rclpy import logging +from construct import ConstError from rclpy.node import Node -from std_msgs.msg import Bool -from game_controller_humanoid.gamestate import GameState, ReturnData, GAME_CONTROLLER_RESPONSE_VERSION -from bitbots_msgs.msg import GameState as GameStateMsg -from bitbots_utils.utils import get_parameters_from_other_node +from rclpy.time import Time +from rclpy.duration import Duration +from std_msgs.msg import Header +from construct import Container +from game_controller_humanoid.gamestate import GameStateStruct, ResponseStruct +from game_controller_humanoid.utils import get_parameters_from_other_node -logger = logging.get_logger('game_controller_humanoid') +from game_controller_humanoid_interfaces.msg import GameState class GameStateReceiver(Node): @@ -37,178 +37,175 @@ class GameStateReceiver(Node): After this we send a package back to the GC """ - def __init__(self): - super().__init__('game_controller', automatically_declare_parameters_from_overrides=True) - - params = get_parameters_from_other_node(self, "parameter_blackboard", ['team_id', 'bot_id']) - self.team_number = params['team_id'] - self.player_number = params['bot_id'] - logger.info('We are playing as player {} in team {}'.format(self.player_number, self.team_number)) - - self.state_publisher = self.create_publisher(GameStateMsg, 'gamestate', 1) - - self.man_penalize = False + def __init__(self, *args, **kwargs): + super().__init__('game_controller', *args, **kwargs, allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) + + # Check if we have the team and bot id parameters or if we should get them from the blackboard + 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 + 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 + 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.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 time in seconds after which we assume the game controller is lost + # and we tell the robot to move self.game_controller_lost_time = 20 - self.game_controller_connected_publisher = self.create_publisher(Bool, 'game_controller_connected', 1) # The address listening on and the port for sending back the robots meta data - listen_host = self.get_parameter('listen_host').value - listen_port = self.get_parameter('listen_port').value - self.addr = (listen_host, listen_port) + self.addr = ( + self.get_parameter('listen_host').value, + self.get_parameter('listen_port').value + ) self.answer_port = self.get_parameter('answer_port').value - # The state and time we received last form the GC - self.state = None - self.time = time.time() + # The time of the last package + self.last_package_time: Time = self.get_clock().now() - # The socket and whether it is still running - self.socket = None - self.running = True + # Create the socket we want to use for the communications + self.socket = self._open_socket() - self._open_socket() - - def _open_socket(self): + def _open_socket(self) -> socket.socket: """ Creates the socket """ self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) self.socket.bind(self.addr) self.socket.settimeout(2) - self.socket2 = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) - self.socket2.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) def receive_forever(self): - """ Waits in a loop that is terminated by setting self.running = False """ - while True: - try: - self.receive_once() - except IOError as e: - logger.warn("Error while sending keepalive: " + str(e)) - - def receive_once(self): - """ Receives a package and interprets it. - Calls :func:`on_new_gamestate` - Sends an answer to the GC """ + """ Waits in a loop for new packages """ + 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 and if so + # call the fallback behavior + if self.get_time_since_last_package() > Duration(seconds=self.game_controller_lost_time): + self.fallback_behavior() + + def receive_and_answer_once(self): + """ Receives a package, interprets it and sends an answer. """ try: - data, peer = self.socket.recvfrom(GameState.sizeof()) + # Receive the package + data, peer = self.socket.recvfrom(GameStateStruct.sizeof()) - # Throws a ConstError if it doesn't work - parsed_state = GameState.parse(data) + # Parse the package based on the GameStateStruct + # 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 - self.state = parsed_state - self.time = time.time() - - # Publish that game controller received message - msg = Bool() - msg.data = True - self.game_controller_connected_publisher.publish(msg) + self.last_package_time = self.get_clock().now() - # Call the handler for the package - self.on_new_gamestate(self.state) + # Build the game state message and publish it + self.state_publisher.publish(self.build_game_state_msg(parsed_state)) # Answer the GameController self.answer_to_gamecontroller(peer) except AssertionError as ae: - logger.error(ae) + self.get_logger().error(ae) except socket.timeout: - logger.info("No GameController message received (socket timeout)", throttle_duration_sec=5) + self.get_logger().info("No GameController message received (socket timeout)", throttle_duration_sec=5) except ConstError: - logger.warn("Parse Error: Probably using an old protocol!") - finally: - if self.get_time_since_last_package() > self.game_controller_lost_time: - self.time += 5 # Resend message every five seconds - logger.info("No GameController message received, allowing robot to move", throttle_duration_sec=5) - msg = GameStateMsg() - msg.game_state = 3 # PLAYING - self.state_publisher.publish(msg) - msg2 = Bool() - msg2.data = False - self.game_controller_connected_publisher.publish(msg2) + self.get_logger().warn("Parse Error: Probably using an old protocol!") + except IOError as e: + self.get_logger().warn(f"Error while sending keep-alive: {str(e)}") + + def fallback_behavior(self): + """ + This is called if we didn't receive a package for a long time. + It tells the robot to play anyway. + """ + self.get_logger().info("No GameController message received, allowing robot to move anyway", throttle_duration_sec=5) + self.state_publisher.publish( + GameState( + header=Header(stamp=self.get_clock().now().to_msg()), + game_state = GameState.GAMESTATE_PLAYING + ) + ) def answer_to_gamecontroller(self, peer): """ Sends a life sign to the game controller """ - return_message = 0 if self.man_penalize else 2 - - data = Container(header=b"RGrt", - version=GAME_CONTROLLER_RESPONSE_VERSION, - team=self.team_number, - player=self.player_number, - message=return_message) + # Build the answer package + data = ResponseStruct.build(dict( + team=self.team_number, + player=self.player_number, + message=2)) + # Send the package + self.get_logger().debug(f'Sending answer to {peer[0]}:{self.answer_port}') try: - destination = peer[0], self.answer_port - logger.debug('Sending answer to {} port {}'.format(destination[0], destination[1])) - self.socket.sendto(ReturnData.build(data), destination) + self.socket.sendto(data, (peer[0], self.answer_port)) except Exception as e: - logger.error("Network Error: %s" % str(e)) - - def on_new_gamestate(self, state): - """ Is called with the new game state after receiving a package. - The information is processed and published as a standard message to a ROS topic. - :param state: Game State - """ - - is_own_team = lambda number: number == self.team_number - own_team = self.select_team_by(is_own_team, state.teams) - - is_not_own_team = lambda number: number != self.team_number - rival_team = self.select_team_by(is_not_own_team, state.teams) - - if not own_team or not rival_team: - logger.error('Team {} not playing, only {} and {}'.format(self.team_number, state.teams[0].team_number, - state.teams[1].team_number)) - return - - try: - me = own_team.players[self.player_number - 1] - except IndexError: - logger.error('Robot {} not playing'.format(self.player_number)) - return - - msg = GameStateMsg() - msg.header.stamp = self.get_clock().now().to_msg() - msg.game_state = state.game_state.intvalue - msg.secondary_state = state.secondary_state.intvalue - msg.secondary_state_mode = state.secondary_state_info[1] - msg.first_half = state.first_half - msg.own_score = own_team.score - msg.rival_score = rival_team.score - msg.seconds_remaining = state.seconds_remaining - msg.secondary_seconds_remaining = state.secondary_seconds_remaining - msg.has_kick_off = state.kick_of_team == self.team_number - msg.penalized = me.penalty != 0 - msg.seconds_till_unpenalized = me.secs_till_unpenalized - msg.secondary_state_team = state.secondary_state_info[0] - msg.secondary_state_mode = state.secondary_state_info[1] - msg.team_color = own_team.team_color.intvalue - msg.drop_in_team = state.drop_in_team - msg.drop_in_time = state.drop_in_time - msg.penalty_shot = own_team.penalty_shot - msg.single_shots = own_team.single_shots - msg.coach_message = own_team.coach_message - penalties = [] - red_cards = [] - for i in range(6): - penalties.append(own_team.players[i].penalty != 0) - red_cards.append(own_team.players[i].number_of_red_cards != 0) - msg.team_mates_with_penalty = penalties - msg.team_mates_with_red_card = red_cards - self.state_publisher.publish(msg) - - def get_last_state(self): - return self.state, self.time - - def get_time_since_last_package(self): - return time.time() - self.time - - def stop(self): - self.running = False - - def set_manual_penalty(self, flag): - self.man_penalize = flag - - def select_team_by(self, predicate, teams): - selected = [team for team in teams if predicate(team.team_number)] + 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 """ + + # 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) + + # 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}' + + assert self.player_number <= len(own_team.players), \ + f'Robot {self.player_number} not playing' + + this_robot = own_team.players[self.player_number - 1] + + return GameState( + header = Header(stamp=self.get_clock().now().to_msg()), + game_state = state.game_state.intvalue, + secondary_state = state.secondary_state.intvalue, + secondary_state_mode = state.secondary_state_info[1], + first_half = state.first_half, + own_score = own_team.score, + rival_score = rival_team.score, + seconds_remaining = state.seconds_remaining, + secondary_seconds_remaining = state.secondary_seconds_remaining, + has_kick_off = state.kick_of_team == self.team_number, + penalized = this_robot.penalty != 0, + seconds_till_unpenalized = this_robot.secs_till_unpenalized, + secondary_state_team = state.secondary_state_info[0], + team_color = own_team.team_color.intvalue, + 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, + coach_message = own_team.coach_message, + team_mates_with_penalty = [player.penalty != 0 for player in own_team.players], + team_mates_with_red_card = [player.number_of_red_cards != 0 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 + + @staticmethod + def select_team_by(predicate: callable, teams: list[Container]) -> Container: + """ Selects the team based on the predicate """ + selected = [team for team in teams if predicate(team)] return next(iter(selected), None) diff --git a/game_controller_humanoid/game_controller_humanoid/utils.py b/game_controller_humanoid/game_controller_humanoid/utils.py new file mode 100644 index 0000000..58a24d5 --- /dev/null +++ b/game_controller_humanoid/game_controller_humanoid/utils.py @@ -0,0 +1,32 @@ +from typing import Any + +import rclpy +from rclpy.node import Node +from rcl_interfaces.srv import GetParameters +from rclpy.parameter import parameter_value_to_python + + +def get_parameters_from_other_node(own_node: Node, + other_node_name: str, + parameter_names: list[str], + service_timeout_sec: float = 20.0) -> dict[str, Any]: + """ + 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') + request = GetParameters.Request() + request.names = parameter_names + future = client.call_async(request) + rclpy.spin_until_future_complete(own_node, future) + response = future.result() + + 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 diff --git a/game_controller_humanoid/package.xml b/game_controller_humanoid/package.xml index f016f3d..9ed4dad 100644 --- a/game_controller_humanoid/package.xml +++ b/game_controller_humanoid/package.xml @@ -14,22 +14,17 @@ Timon Engelke - MIT + Apache License 2.0 rosidl_default_generators rosidl_default_runtime - bitbots_docs - bitbots_msgs + game_controller_humanoid_interfaces python3-construct rclpy std_msgs ament_python - - tested_integration - python - diff --git a/game_controller_humanoid/setup.py b/game_controller_humanoid/setup.py index b240467..41d2c1a 100644 --- a/game_controller_humanoid/setup.py +++ b/game_controller_humanoid/setup.py @@ -1,7 +1,6 @@ import glob -from setuptools import find_packages -from setuptools import setup +from setuptools import find_packages, setup package_name = 'game_controller_humanoid' @@ -26,6 +25,7 @@ zip_safe=True, keywords=['ROS'], license='MIT', + tests_require=['pytest'], entry_points={ 'console_scripts': [ 'game_controller = game_controller_humanoid.receiver:main', diff --git a/game_controller_humanoid/test/test_game_controller.py b/game_controller_humanoid/test/test_game_controller.py new file mode 100644 index 0000000..407801a --- /dev/null +++ b/game_controller_humanoid/test/test_game_controller.py @@ -0,0 +1,220 @@ + +import rclpy + +from construct import Container, ListContainer, EnumIntegerString + +from rclpy.node import Node +from rclpy.parameter import Parameter + +from game_controller_humanoid.gamestate import GameStateStruct, TeamInfoStruct +from game_controller_humanoid.utils import get_parameters_from_other_node +from game_controller_humanoid.receiver import GameStateReceiver + +from game_controller_humanoid_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, + number_of_yellow_cards=0, + number_of_red_cards=0, + number_of_warnings=0, + goalkeeper=False + ) + + # Create a TeamInfoStruct as if it was a parsed game state + team = TeamInfoStruct.build(dict( + team_number=1, + team_color=0, + score=0, + penalty_shot=0, + single_shots=0, + coach_sequence=0, + coach_message='', + coach=player, + 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, + game_type=0, + game_state=0, + first_half=False, + kick_of_team=0, + secondary_state=0, + secondary_state_info=b'\x00\x00\x00\x00', + drop_in_team=0, + drop_in_time=0, + seconds_remaining=0, + secondary_seconds_remaining=0, + teams=[dict( + team_number=team_id, + team_color=0, + score=0, + penalty_shot=0, + single_shots=0, + coach_sequence=0, + coach_message='', + coach=dict( + penalty=0, + secs_till_unpenalized=0, + number_of_yellow_cards=0, + number_of_red_cards=0, + number_of_warnings=0, + goalkeeper=False + ), + players=[dict( + penalty=0, + secs_till_unpenalized=0, + number_of_yellow_cards=0, + number_of_red_cards=0, + number_of_warnings=0, + goalkeeper=not bool(player_id) + ) 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 + 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, + game_type=0, + game_state=0, + first_half=False, + kick_of_team=0, + secondary_state=0, + secondary_state_info=b'\x00\x00\x00\x00', + drop_in_team=0, + drop_in_time=0, + seconds_remaining=0, + secondary_seconds_remaining=0, + teams=[dict( + team_number=team_id, + team_color=0, + score=0, + penalty_shot=0, + single_shots=0, + coach_sequence=0, + coach_message='', + coach=dict( + penalty=0, + secs_till_unpenalized=0, + number_of_yellow_cards=0, + number_of_red_cards=0, + number_of_warnings=0, + goalkeeper=False + ), + players=[dict( + penalty=0, + secs_till_unpenalized=0, + number_of_yellow_cards=0, + number_of_red_cards=0, + number_of_warnings=0, + goalkeeper=not bool(player_id) + ) 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.game_state == 0 + assert msg.secondary_state == 0 + assert msg.secondary_state_mode == 0 + assert msg.first_half == False + assert msg.own_score == 0 + assert msg.rival_score == 0 + assert msg.seconds_remaining == 0 + assert msg.secondary_seconds_remaining == 0 + assert msg.has_kick_off == False + assert msg.penalized == False + assert msg.seconds_till_unpenalized == 0 + assert msg.secondary_state_team == 0 + assert msg.team_color == 0 + assert msg.drop_in_team == 0 + assert msg.drop_in_time == 0 + assert msg.penalty_shot == 0 + assert msg.single_shots == 0 + assert msg.coach_message == '' + assert msg.team_mates_with_penalty == [False] * num_players + assert msg.team_mates_with_red_card == [False] * num_players + + diff --git a/game_controller_humanoid_interfaces/.gitignore b/game_controller_humanoid_interfaces/.gitignore new file mode 100644 index 0000000..1716c0f --- /dev/null +++ b/game_controller_humanoid_interfaces/.gitignore @@ -0,0 +1,12 @@ +# auto-generated documentation +**/docs/_build +**/docs/_out +**/docs/cppapi +**/docs/pyapi + +.idea/* +*.pyc +.pyenv2/* +.pyenv3/* +.vscode/* + diff --git a/game_controller_humanoid_interfaces/CMakeLists.txt b/game_controller_humanoid_interfaces/CMakeLists.txt new file mode 100644 index 0000000..b0cd129 --- /dev/null +++ b/game_controller_humanoid_interfaces/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.5) +project(game_controller_humanoid_interfaces) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/GameState.msg" + DEPENDENCIES + std_msgs +) + +ament_package() diff --git a/game_controller_humanoid_interfaces/msg/GameState.msg b/game_controller_humanoid_interfaces/msg/GameState.msg new file mode 100644 index 0000000..f970d7d --- /dev/null +++ b/game_controller_humanoid_interfaces/msg/GameState.msg @@ -0,0 +1,67 @@ +# This message provides all information from the game controller +# for additional information see documentation of the game controller +# https://github.com/bhuman/GameController + + +std_msgs/Header header + +uint8 GAMESTATE_INITIAL=0 +uint8 GAMESTATE_READY=1 +uint8 GAMESTATE_SET=2 +uint8 GAMESTATE_PLAYING=3 +uint8 GAMESTATE_FINISHED=4 +uint8 game_state + +# Secondary state, penaltyshoot is penalty shootout at the end of the game, +# penaltykick is a kick during the game +uint8 STATE_NORMAL = 0 +uint8 STATE_PENALTYSHOOT = 1 +uint8 STATE_OVERTIME = 2 +uint8 STATE_TIMEOUT = 3 +uint8 STATE_DIRECT_FREEKICK = 4 +uint8 STATE_INDIRECT_FREEKICK = 5 +uint8 STATE_PENALTYKICK = 6 +uint8 STATE_CORNER_KICK = 7 +uint8 STATE_GOAL_KICK = 8 +uint8 STATE_THROW_IN = 9 +uint8 secondary_state + +# For newest version of game controller +# Tells which team has the free kick or penalty kick +uint8 secondary_state_team +# The secondary state contains a sub mode in which phase of execution the secondary state is +uint8 MODE_PREPARATION = 0 +uint8 MODE_PLACING = 1 +uint8 MODE_END = 2 +uint8 secondary_state_mode + +bool first_half +uint8 own_score +uint8 rival_score + +# Seconds remaining for the game half +int16 seconds_remaining +# Seconds remaining for things like kickoff +int16 secondary_seconds_remaining + +bool has_kick_off +bool penalized +uint16 seconds_till_unpenalized + +# Team colors +uint8 BLUE = 0 +uint8 RED = 1 +uint8 team_color + +bool drop_in_team +uint16 drop_in_time + +# The number of the current penalty shot during penalty shootout +uint8 penalty_shot +# a binary pattern indicating the successful penalty shots (1 for successful, 0 for unsuccessful) +uint16 single_shots + +string coach_message + +bool[] team_mates_with_penalty +bool[] team_mates_with_red_card diff --git a/game_controller_humanoid_interfaces/package.xml b/game_controller_humanoid_interfaces/package.xml new file mode 100644 index 0000000..bf2a01b --- /dev/null +++ b/game_controller_humanoid_interfaces/package.xml @@ -0,0 +1,27 @@ + + + + game_controller_humanoid_interfaces + 0.0.1 + RoboCup Humanoid League Game Controller Messages + + Florian Vahl + Hamburg Bit-Bots + + Apache License 2.0 + + Marc Bestmann + + ament_cmake + rosidl_default_generators + + std_msgs + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + +