Copy game controller

.gitignore

@@ -0,0 +1,6 @@
+__pycache__
+*.pyc
+humanoid_league_speaker/cfg/cpp/
+humanoid_league_speaker/src/humanoid_league_speaker/cfg/
+.vscode
+```

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

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

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
+)

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()

game_controller_humanoid/launch/game_controller.launch

@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<launch>
+    <arg name="sim" default="false" />
+
+    <node pkg="game_controller_humanoid" exec="game_controller" name="game_controller_humanoid" output="screen">
+        <param from="$(find-pkg-share game_controller_humanoid)/config/game_controller_settings.yaml" />
+        <param name="use_sim_time" value="$(var sim)" />
+    </node>
+</launch>