Merge pull request #61 from ros-sports/mergify/bp/humble/pr-58

Big refactor 2 (backport #58)

.gitignore

@@ -0,0 +1 @@
+__pycache__

gc_spl/gc_spl/gc_spl.py

@@ -0,0 +1,148 @@
+# Copyright 2022 Kenji Brameld
+#
+# 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
+from threading import Thread
+
+import rclpy
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+
+
+class GCSPL(Node):
+    """Node that runs on the robot to communicate with SPL GameController."""
+
+    _loop_thread = None
+    _client = None
+    _publisher = None
+    _host = None
+    _RCGCD = None
+
+    def __init__(self, node_name='gc_spl', **kwargs):
+        super().__init__(node_name, **kwargs)
+
+        # Declare parameters
+        self.declare_parameter('return_port', 3939)
+        self.declare_parameter('rcgcd_version', Parameter.Type.INTEGER)
+        self.declare_parameter('rcgcrd_version', Parameter.Type.INTEGER)
+
+        # Read and log parameters
+        return_port = self.get_parameter('return_port').value
+        self.get_logger().debug('return_port: %s' % return_port)
+
+        # Import libraries dependending on rcgcd and rcgcrd version
+        rcgcd_version = self.get_parameter('rcgcd_version').value
+        if rcgcd_version is None:
+            self.get_logger().error('rcgcd_version not provided as a parameter. ' +
+                                    'Unable to start node.')
+            return
+        self.get_logger().debug('rcgcd_version: %s' % rcgcd_version)
+
+        rcgcrd_version = self.get_parameter('rcgcrd_version').value
+        if rcgcrd_version is None:
+            self.get_logger().error('rcgcrd_version not provided as a parameter. ' +
+                                    'Unable to start node.')
+            return
+        self.get_logger().debug('rcgcrd_version: %s' % rcgcrd_version)
+
+        self._setup_methods(rcgcd_version, rcgcrd_version)
+
+        # Setup publisher
+        self._publisher = self.create_publisher(self.RCGCD, 'gc/data', 10)
+
+        # Setup subscriber
+        self._subscriber = self.create_subscription(
+            self.RCGCRD, 'gc/return_data', self._rcgcrd_callback, 10)
+
+        # UDP Client - adapted from https://github.com/ninedraft/python-udp/blob/master/client.py
+        self._client = socket.socket(
+            socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)  # UDP
+        # This has to be SO_REUSEADDR instead of SO_REUSEPORT to work with TCM
+        self._client.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        self._client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+        self._client.bind(('', self.GAMECONTROLLER_DATA_PORT))
+        # Set timeout so _loop can constantly check for rclpy.ok()
+        self._client.settimeout(0.1)
+
+        # Start thread to continuously poll
+        self._loop_thread = Thread(target=self._loop)
+        self._loop_thread.start()
+
+    def _setup_methods(self, rcgcd_version, rcgcrd_version):
+        # RCGCD
+        if rcgcd_version == 14:
+            from gc_spl_interfaces.msg import RCGCD14 as RCGCD
+            from gc_spl.rcgcd_14.robocup_game_control_data import \
+                GAMECONTROLLER_DATA_PORT
+            from gc_spl.rcgcd_14.conversion import rcgcd_data_to_msg
+        elif rcgcd_version == 15:
+            from gc_spl_interfaces.msg import RCGCD15 as RCGCD
+            from gc_spl.rcgcd_15.robocup_game_control_data import \
+                GAMECONTROLLER_DATA_PORT
+            from gc_spl.rcgcd_15.conversion import rcgcd_data_to_msg
+        else:
+            self.get_logger().error('rcgcd_version ' + rcgcd_version + ' is not supported.')
+        self.RCGCD = RCGCD
+        self.GAMECONTROLLER_DATA_PORT = GAMECONTROLLER_DATA_PORT
+        self.rcgcd_data_to_msg = rcgcd_data_to_msg
+
+        # RCGCRD
+        if rcgcrd_version == 4:
+            from gc_spl_interfaces.msg import RCGCRD4 as RCGCRD
+            from gc_spl.rcgcrd_4.conversion import rcgcrd_msg_to_data
+            from gc_spl.rcgcrd_4.robocup_game_control_return_data import \
+                GAMECONTROLLER_RETURN_PORT
+        else:
+            self.get_logger().error('rcgcrd_version ' + rcgcrd_version + ' is not supported.')
+        self.RCGCRD = RCGCRD
+        self.rcgcrd_msg_to_data = rcgcrd_msg_to_data
+        self.GAMECONTROLLER_RETURN_PORT = GAMECONTROLLER_RETURN_PORT
+
+    def _loop(self):
+        while rclpy.ok():
+            try:
+                data, (self._host, _) = self._client.recvfrom(1024)
+                self.get_logger().debug('received: "%s"' % data)
+
+                # Convert data to ROS msg
+                msg = self.rcgcd_data_to_msg(data)
+
+                # Publish it
+                self._publisher.publish(msg)
+            except TimeoutError:
+                pass
+
+    def _rcgcrd_callback(self, msg):
+
+        if self._host is None:
+            self.get_logger().debug(
+                'Not returning RoboCupGameControlReturnData, as GameController'
+                ' host address is not known yet.')
+            return
+
+        data = self.rcgcrd_msg_to_data(msg)
+
+        # Return data directly to the GameController's address and return port
+        self._client.sendto(data, (self._host, self.GAMECONTROLLER_RETURN_PORT))
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    gc_spl = GCSPL()
+    rclpy.spin(gc_spl)
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

gc_spl/gc_spl/rcgcd_14/conversion.py

@@ -0,0 +1,46 @@
+# Copyright 2022 Kenji Brameld
+#
+# 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.
+
+from gc_spl.rcgcd_14.robocup_game_control_data import (
+    MAX_NUM_PLAYERS, RoboCupGameControlData)
+from gc_spl_interfaces.msg import RCGCD14
+
+
+def rcgcd_data_to_msg(data: bytes) -> RCGCD14:
+    """Convert binary data to RCGCRD ROS msg."""
+    parsed = RoboCupGameControlData.parse(data)
+    msg = RCGCD14()
+    msg.packet_number = parsed.packetNumber
+    msg.players_per_team = parsed.playersPerTeam
+    msg.competition_phase = parsed.competitionPhase
+    msg.competition_type = parsed.competitionType
+    msg.game_phase = parsed.gamePhase
+    msg.state = parsed.state
+    msg.set_play = parsed.setPlay
+    msg.first_half = parsed.firstHalf
+    msg.kicking_team = parsed.kickingTeam
+    msg.secs_remaining = parsed.secsRemaining
+    msg.secondary_time = parsed.secondaryTime
+    for t in range(2):
+        msg.teams[t].team_number = parsed.teams[t].teamNumber
+        msg.teams[t].team_colour = parsed.teams[t].teamColour
+        msg.teams[t].score = parsed.teams[t].score
+        msg.teams[t].penalty_shot = parsed.teams[t].penaltyShot
+        msg.teams[t].single_shots = parsed.teams[t].singleShots
+        msg.teams[t].message_budget = parsed.teams[t].messageBudget
+        for p in range(MAX_NUM_PLAYERS):
+            msg.teams[t].players[p].penalty = parsed.teams[t].players[p].penalty
+            msg.teams[t].players[p].secs_till_unpenalised = \
+                parsed.teams[t].players[p].secsTillUnpenalised
+    return msg

gc_spl/gc_spl/rcgcd_14/robocup_game_control_data.py

@@ -0,0 +1,135 @@
+# Copyright 2022 Kenji Brameld
+#
+# 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.
+
+"""
+Library to convert binary udp data to a meaningful python object.
+
+Example of listening to the GameController, and accessing the data using this library::
+
+    from robocup_game_control_data import GAMECONTROLLER_DATA_PORT, RoboCupGameControlData
+    import socket
+
+    # Setup UDP client
+    client = socket.socket(
+        socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)  # UDP
+    client.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)  # SO_REUSEADDR instead of SO_REUSEPORT to work while TCM is running  # noqa: E501
+    client.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+    client.bind(('', GAMECONTROLLER_DATA_PORT))
+
+    # Receive data
+    data = client.recv(1024)
+
+    # Parse it
+    parsed = RoboCupGameControlData.parse(data)
+
+    # Accessing data
+    print('First team player4 penalty: ',
+        parsed.teams[0].players[3].penalty)
+    print('state: ', parsed.state)
+"""
+
+
+from construct import Array, Byte, Const, Int16sl, Int16ul, Struct
+
+GAMECONTROLLER_DATA_PORT = 3838
+
+GAMECONTROLLER_STRUCT_HEADER = b'RGme'
+GAMECONTROLLER_STRUCT_VERSION = 14
+
+MAX_NUM_PLAYERS = 7
+
+# SPL
+TEAM_BLUE = 0  # blue, cyan
+TEAM_RED = 1  # red, magenta, pink
+TEAM_YELLOW = 2  # yellow
+TEAM_BLACK = 3  # black, dark gray
+TEAM_WHITE = 4  # white
+TEAM_GREEN = 5  # green
+TEAM_ORANGE = 6  # orange
+TEAM_PURPLE = 7  # purple, violet
+TEAM_BROWN = 8  # brown
+TEAM_GRAY = 9  # lighter gray
+
+COMPETITION_PHASE_ROUNDROBIN = 0
+COMPETITION_PHASE_PLAYOFF = 1
+
+COMPETITION_TYPE_NORMAL = 0
+COMPETITION_TYPE_CHALLENGE_SHIELD = 1
+COMPETITION_TYPE_7V7 = 2
+COMPETITION_TYPE_DYNAMIC_BALL_HANDLING = 3
+
+GAME_PHASE_NORMAL = 0
+GAME_PHASE_PENALTYSHOOT = 1
+GAME_PHASE_OVERTIME = 2
+GAME_PHASE_TIMEOUT = 3
+
+STATE_INITIAL = 0
+STATE_READY = 1
+STATE_SET = 2
+STATE_PLAYING = 3
+STATE_FINISHED = 4
+
+SET_PLAY_NONE = 0
+SET_PLAY_GOAL_KICK = 1
+SET_PLAY_PUSHING_FREE_KICK = 2
+SET_PLAY_CORNER_KICK = 3
+SET_PLAY_KICK_IN = 4
+SET_PLAY_PENALTY_KICK = 5
+
+PENALTY_NONE = 0
+# SPL
+PENALTY_SPL_ILLEGAL_BALL_CONTACT = 1   # ball holding / playing with hands
+PENALTY_SPL_PLAYER_PUSHING = 2
+PENALTY_SPL_ILLEGAL_MOTION_IN_SET = 3  # heard whistle too early?
+PENALTY_SPL_INACTIVE_PLAYER = 4        # fallen, inactive
+PENALTY_SPL_ILLEGAL_POSITION = 5
+PENALTY_SPL_LEAVING_THE_FIELD = 6
+PENALTY_SPL_REQUEST_FOR_PICKUP = 7
+PENALTY_SPL_LOCAL_GAME_STUCK = 8
+PENALTY_SPL_ILLEGAL_POSITION_IN_SET = 9
+
+PENALTY_SUBSTITUTE = 14
+PENALTY_MANUAL = 15
+
+RobotInfo = Struct(
+    'penalty' / Byte,             # penalty state of the player
+    'secsTillUnpenalised' / Byte  # estimate of time till unpenalised
+)
+
+TeamInfo = Struct(
+    'teamNumber' / Byte,        # unique team number
+    'teamColour' / Byte,        # colour of the team
+    'score' / Byte,             # team's score
+    'penaltyShot' / Byte,       # penalty shot counter
+    'singleShots' / Int16ul,    # bits represent penalty shot success  # noqa: E501
+    'messageBudget' / Int16ul,  # number of team messages the team is allowed to send for the remainder of the game  # noqa: E501
+    'players' / Array(MAX_NUM_PLAYERS, RobotInfo)  # the team's players
+)
+
+RoboCupGameControlData = Struct(
+    'header' / Const(GAMECONTROLLER_STRUCT_HEADER),  # header to identify the structure  # noqa: E501
+    'version' / Const(GAMECONTROLLER_STRUCT_VERSION, Byte),  # version of the data structure  # noqa: E501
+    'packetNumber' / Byte,      # number incremented with each packet sent (with wraparound)  # noqa: E501
+    'playersPerTeam' / Byte,    # the number of players on a team
+    'competitionPhase' / Byte,  # phase of the competition (COMPETITION_PHASE_ROUNDROBIN, COMPETITION_PHASE_PLAYOFF)  # noqa: E501
+    'competitionType' / Byte,   # type of the competition (COMPETITION_TYPE_NORMAL, COMPETITION_TYPE_CHALLENGE_SHIELD, etc)  # noqa: E501
+    'gamePhase' / Byte,         # phase of the game (GAME_PHASE_NORMAL, GAME_PHASE_PENALTYSHOOT, etc)  # noqa: E501
+    'state' / Byte,             # state of the game (STATE_READY, STATE_PLAYING, etc)  # noqa: E501
+    'setPlay' / Byte,           # active set play (SET_PLAY_NONE, SET_PLAY_GOAL_KICK, etc)  # noqa: E501
+    'firstHalf' / Byte,         # 1 = game in first half, 0 otherwise
+    'kickingTeam' / Byte,       # the team number of the next team to kick off, free kick etc
+    'secsRemaining' / Int16sl,  # estimate of number of seconds remaining in the half
+    'secondaryTime' / Int16sl,  # number of seconds shown as secondary time (remaining ready, until free ball, etc)  # noqa: E501
+    'teams' / Array(2, TeamInfo)
+)

gc_spl/gc_spl/rcgcd_15/conversion.py

@@ -0,0 +1,48 @@
+# Copyright 2022 Kenji Brameld
+#
+# 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.
+
+from gc_spl.rcgcd_15.robocup_game_control_data import (
+    MAX_NUM_PLAYERS, RoboCupGameControlData)
+from gc_spl_interfaces.msg import RCGCD15
+
+
+def rcgcd_data_to_msg(data: bytes) -> RCGCD15:
+    """Convert binary data to RCGCRD ROS msg."""
+    parsed = RoboCupGameControlData.parse(data)
+    msg = RCGCD15()
+    msg.packet_number = parsed.packetNumber
+    msg.players_per_team = parsed.playersPerTeam
+    msg.competition_phase = parsed.competitionPhase
+    msg.competition_type = parsed.competitionType
+    msg.game_phase = parsed.gamePhase
+    msg.state = parsed.state
+    msg.set_play = parsed.setPlay
+    msg.first_half = parsed.firstHalf
+    msg.kicking_team = parsed.kickingTeam
+    msg.secs_remaining = parsed.secsRemaining
+    msg.secondary_time = parsed.secondaryTime
+    for t in range(2):
+        msg.teams[t].team_number = parsed.teams[t].teamNumber
+        msg.teams[t].field_player_colour = parsed.teams[t].fieldPlayerColour
+        msg.teams[t].goalkeeper_colour = parsed.teams[t].goalkeeperColour
+        msg.teams[t].goalkeeper = parsed.teams[t].goalkeeper
+        msg.teams[t].score = parsed.teams[t].score
+        msg.teams[t].penalty_shot = parsed.teams[t].penaltyShot
+        msg.teams[t].single_shots = parsed.teams[t].singleShots
+        msg.teams[t].message_budget = parsed.teams[t].messageBudget
+        for p in range(MAX_NUM_PLAYERS):
+            msg.teams[t].players[p].penalty = parsed.teams[t].players[p].penalty
+            msg.teams[t].players[p].secs_till_unpenalised = \
+                parsed.teams[t].players[p].secsTillUnpenalised
+    return msg