diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..127998e --- /dev/null +++ b/.gitignore @@ -0,0 +1,6 @@ +__pycache__ +*.pyc +humanoid_league_speaker/cfg/cpp/ +humanoid_league_speaker/src/humanoid_league_speaker/cfg/ +.vscode +``` \ No newline at end of file diff --git a/README.md b/README.md index c64ee00..692a202 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,9 @@ -# Template Repo +# Game Controller Client [![Build and Test (humble)](../../actions/workflows/build_and_test_humble.yaml/badge.svg?branch=rolling)](../../actions/workflows/build_and_test_humble.yaml?query=branch:rolling) [![Build and Test (iron)](../../actions/workflows/build_and_test_iron.yaml/badge.svg?branch=rolling)](../../actions/workflows/build_and_test_iron.yaml?query=branch:rolling) [![Build and Test (rolling)](../../actions/workflows/build_and_test_rolling.yaml/badge.svg?branch=rolling)](../../actions/workflows/build_and_test_rolling.yaml?query=branch:rolling) + +This is the client for the humanoid league game controller. It communicates with the game controller via UDP and sends a game state message to ROS subscribers. + +## Still work in progress \ No newline at end of file diff --git a/game_controller_humanoid/config/game_controller_settings.yaml b/game_controller_humanoid/config/game_controller_settings.yaml new file mode 100644 index 0000000..77fecb4 --- /dev/null +++ b/game_controller_humanoid/config/game_controller_settings.yaml @@ -0,0 +1,8 @@ +--- +# Game controller network settings + +game_controller_humanoid: + ros__parameters: + listen_host: '0.0.0.0' + listen_port: 3838 + answer_port: 3939 diff --git a/game_controller_humanoid/game_controller_humanoid/__init__.py b/game_controller_humanoid/game_controller_humanoid/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/game_controller_humanoid/game_controller_humanoid/gamestate.py b/game_controller_humanoid/game_controller_humanoid/gamestate.py new file mode 100644 index 0000000..b695e6e --- /dev/null +++ b/game_controller_humanoid/game_controller_humanoid/gamestate.py @@ -0,0 +1,98 @@ +#!/usr/bin/env python +# -*- coding:utf-8 -*- + +from construct import Byte, Struct, Enum, Bytes, Const, Array, Int16ul, PaddedString, Flag, Int16sl + +Short = Int16ul + +RobotInfo = "robot_info" / Struct( + # define NONE 0 + # define PENALTY_HL_KID_BALL_MANIPULATION 1 + # define PENALTY_HL_KID_PHYSICAL_CONTACT 2 + # define PENALTY_HL_KID_ILLEGAL_ATTACK 3 + # define PENALTY_HL_KID_ILLEGAL_DEFENSE 4 + # define PENALTY_HL_KID_REQUEST_FOR_PICKUP 5 + # define PENALTY_HL_KID_REQUEST_FOR_SERVICE 6 + # define PENALTY_HL_KID_REQUEST_FOR_PICKUP_2_SERVICE 7 + # define MANUAL 15 + "penalty" / Byte, + "secs_till_unpenalized" / Byte, + "number_of_warnings" / Byte, + "number_of_yellow_cards" / Byte, + "number_of_red_cards" / Byte, + "goalkeeper" / Flag +) + +TeamInfo = "team" / Struct( + "team_number" / Byte, + "team_color" / Enum(Byte, + BLUE=0, + RED=1, + YELLOW=2, + BLACK=3, + WHITE=4, + GREEN=5, + ORANGE=6, + PURPLE=7, + BROWN=8, + GRAY=9 + ), + "score" / Byte, + "penalty_shot" / Byte, # penalty shot counter + "single_shots" / Short, # bits represent penalty shot success + "coach_sequence" / Byte, + "coach_message" / PaddedString(253, 'utf8'), + "coach" / RobotInfo, + "players" / Array(11, RobotInfo) +) + +GameState = "gamedata" / Struct( + "header" / Const(b'RGme'), + "version" / Const(12, Short), + "packet_number" / Byte, + "players_per_team" / Byte, + "game_type" / Byte, + "game_state" / Enum(Byte, + STATE_INITIAL=0, + # auf startposition gehen + STATE_READY=1, + # bereithalten + STATE_SET=2, + # spielen + STATE_PLAYING=3, + # spiel zu ende + STATE_FINISHED=4 + ), + "first_half" / Flag, + "kick_of_team" / Byte, + "secondary_state" / Enum(Byte, + STATE_NORMAL=0, + STATE_PENALTYSHOOT=1, + STATE_OVERTIME=2, + STATE_TIMEOUT=3, + STATE_DIRECT_FREEKICK=4, + STATE_INDIRECT_FREEKICK=5, + STATE_PENALTYKICK=6, + STATE_CORNERKICK=7, + STATE_GOALKICK=8, + STATE_THROWIN=9, + DROPBALL=128, + UNKNOWN=255 + ), + "secondary_state_info" / Bytes(4), + "drop_in_team" / Flag, + "drop_in_time" / Short, + "seconds_remaining" / Int16sl, + "secondary_seconds_remaining" / Int16sl, + "teams" / Array(2, "team" / TeamInfo) +) + +GAME_CONTROLLER_RESPONSE_VERSION = 2 + +ReturnData = Struct( + "header" / Const(b"RGrt"), + "version" / Const(2, 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 new file mode 100755 index 0000000..e90a8bb --- /dev/null +++ b/game_controller_humanoid/game_controller_humanoid/receiver.py @@ -0,0 +1,227 @@ +# Copyright (c) 2023 Hamburg Bit-Bots +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +import socket +import time +import rclpy + +from construct import Container, ConstError +from rclpy import logging +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 + +logger = logging.get_logger('game_controller_humanoid') + + +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. + + 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. + + 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 + 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.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 socket and whether it is still running + self.socket = None + self.running = True + + self._open_socket() + + def _open_socket(self): + """ 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 """ + try: + data, peer = self.socket.recvfrom(GameState.sizeof()) + + # Throws a ConstError if it doesn't work + parsed_state = GameState.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) + + # Call the handler for the package + self.on_new_gamestate(self.state) + + # Answer the GameController + self.answer_to_gamecontroller(peer) + + except AssertionError as ae: + logger.error(ae) + except socket.timeout: + 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) + + 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) + 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) + 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)] + return next(iter(selected), None) + + +def main(args=None): + rclpy.init(args=args) + receiver = GameStateReceiver() + + try: + receiver.receive_forever() + except KeyboardInterrupt: + receiver.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/game_controller_humanoid/launch/game_controller.launch b/game_controller_humanoid/launch/game_controller.launch new file mode 100644 index 0000000..2fc2d72 --- /dev/null +++ b/game_controller_humanoid/launch/game_controller.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/game_controller_humanoid/package.xml b/game_controller_humanoid/package.xml new file mode 100644 index 0000000..f016f3d --- /dev/null +++ b/game_controller_humanoid/package.xml @@ -0,0 +1,35 @@ + + + + game_controller_humanoid + 1.1.0 + + The game_controller_humanoid packages receives packets from the GameController and + republishes them as bitbots_msgs/GameState ROS messages. It sends response packets + back to the GameController. + + + Timon Engelke + Hamburg Bit-Bots + + Timon Engelke + + MIT + + rosidl_default_generators + rosidl_default_runtime + + bitbots_docs + bitbots_msgs + python3-construct + rclpy + std_msgs + + + ament_python + + tested_integration + python + + + diff --git a/game_controller_humanoid/resource/game_controller_humanoid b/game_controller_humanoid/resource/game_controller_humanoid new file mode 100644 index 0000000..e69de29 diff --git a/game_controller_humanoid/scripts/sim_gamestate.py b/game_controller_humanoid/scripts/sim_gamestate.py new file mode 100755 index 0000000..dbaf9cc --- /dev/null +++ b/game_controller_humanoid/scripts/sim_gamestate.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 + +# This script was based on the teleop_twist_keyboard package +# original code can be found at https://github.com/ros-teleop/teleop_twist_keyboard +# 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 termios +import tty + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, DurabilityPolicy +from bitbots_msgs.msg import GameState as GameStateMsg +from bitbots_utils.utils import get_parameters_from_other_node + + +class SimGamestate(Node): + msg = """Setting the GameState by entering a number: +0: GAMESTATE_INITIAL=0 +1: GAMESTATE_READY=1 +2: GAMESTATE_SET=2 +3: GAMESTATE_PLAYING=3 +4: GAMESTATE_FINISHED=4 + +Set the secondary game state by entering: +a: STATE_NORMAL = 0 +b: STATE_PENALTYSHOOT = 1 +c: STATE_OVERTIME = 2 +d: STATE_TIMEOUT = 3 +e: STATE_DIRECT_FREEKICK = 4 +f: STATE_INDIRECT_FREEKICK = 5 +g: STATE_PENALTYKICK = 6 +h: STATE_CORNER_KICK = 7 +i: STATE_GOAL_KICK = 8 +j: STATE_THROW_IN = 9 + +p: toggle penalized +t: toggle secondary state team +m: toggle secondary state mode +k: toggle kick off ++: increase own score by 1 + + + + + + + + +CTRL-C to quit +""" + + def __init__(self): + super().__init__("sim_gamestate") + self.logger = self.get_logger() + + self.team_id = get_parameters_from_other_node(self, "parameter_blackboard", ["team_id"])["team_id"] + self.has_kick_off = True + + self.settings = termios.tcgetattr(sys.stdin) + + self.publisher = self.create_publisher( + GameStateMsg, + "gamestate", + QoSProfile(durability=DurabilityPolicy.TRANSIENT_LOCAL, depth=1), + ) + + def loop(self): + game_state_msg = GameStateMsg() + game_state_msg.header.stamp = self.get_clock().now().to_msg() + + # Init secondary state team to our teamID + game_state_msg.secondary_state_team = self.team_id + + try: + print(self.msg) + while True: + key = self.get_key() + if key == "\x03": + break + elif key in ["0", "1", "2", "3", "4"]: + int_key = int(key) + game_state_msg.game_state = int_key + 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(10)]: + game_state_msg.secondary_state = ord(key) - ord("a") + elif key == "m": + game_state_msg.secondary_state_mode = (game_state_msg.secondary_state_mode + 1) % 3 + elif key == "t": + if game_state_msg.secondary_state_team == self.team_id: + game_state_msg.secondary_state_team = self.team_id + 1 + else: + game_state_msg.secondary_state_team = self.team_id + elif key == "k": + self.has_kick_off = not self.has_kick_off + elif key == "+": + game_state_msg.own_score += 1 + game_state_msg.has_kick_off = self.has_kick_off + + 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"""Penalized: {game_state_msg.penalized} +Secondary State Team: {game_state_msg.secondary_state_team} +Secondary State Mode: {game_state_msg.secondary_state_mode} +Secondary State: {game_state_msg.secondary_state} +Gamestate: {game_state_msg.game_state} +Has Kick Off: {game_state_msg.has_kick_off} + + +CTRL-C to quit +""" + ) + + except Exception as e: + print(e) + + finally: + print() + + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) + + def get_key(self): + tty.setraw(sys.stdin.fileno()) + select.select([sys.stdin], [], [], 0) + return_key = sys.stdin.read(1) + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) + return return_key + + +if __name__ == "__main__": + rclpy.init(args=None) + node = SimGamestate() + node.loop() + node.destroy_node() + rclpy.shutdown() diff --git a/game_controller_humanoid/setup.cfg b/game_controller_humanoid/setup.cfg new file mode 100644 index 0000000..7f8929b --- /dev/null +++ b/game_controller_humanoid/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/game_controller_humanoid +[install] +install_scripts=$base/lib/game_controller_humanoid diff --git a/game_controller_humanoid/setup.py b/game_controller_humanoid/setup.py new file mode 100644 index 0000000..b240467 --- /dev/null +++ b/game_controller_humanoid/setup.py @@ -0,0 +1,34 @@ +import glob + +from setuptools import find_packages +from setuptools import setup + +package_name = 'game_controller_humanoid' + +setup( + name=package_name, + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + "/config", + glob.glob('config/*.yaml')), + ('share/' + package_name + '/launch', + glob.glob('launch/*.launch')), + ], + install_requires=[ + 'launch', + 'setuptools', + 'construct', + ], + scripts=['scripts/sim_gamestate.py'], + zip_safe=True, + keywords=['ROS'], + license='MIT', + entry_points={ + 'console_scripts': [ + 'game_controller = game_controller_humanoid.receiver:main', + ], + } +)