diff --git a/.github/workflows/build_and_test_humble.yaml b/.github/workflows/build_and_test_humble.yaml index cdb1596..1363d91 100644 --- a/.github/workflows/build_and_test_humble.yaml +++ b/.github/workflows/build_and_test_humble.yaml @@ -26,9 +26,11 @@ jobs: # Steps represent a sequence of tasks that will be executed as part of the job steps: + - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@v0.7 with: use-ros2-testing: true - uses: ros-tooling/action-ros-ci@v0.3 with: target-ros2-distro: humble + vcs-repo-file-url: dependencies.repos diff --git a/.github/workflows/build_and_test_iron.yaml b/.github/workflows/build_and_test_iron.yaml index b483651..5c653d4 100644 --- a/.github/workflows/build_and_test_iron.yaml +++ b/.github/workflows/build_and_test_iron.yaml @@ -26,9 +26,11 @@ jobs: # Steps represent a sequence of tasks that will be executed as part of the job steps: + - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@v0.7 with: use-ros2-testing: true - uses: ros-tooling/action-ros-ci@v0.3 with: target-ros2-distro: iron + vcs-repo-file-url: dependencies.repos diff --git a/.github/workflows/build_and_test_rolling.yaml b/.github/workflows/build_and_test_rolling.yaml index 7fee75d..2b63ff1 100644 --- a/.github/workflows/build_and_test_rolling.yaml +++ b/.github/workflows/build_and_test_rolling.yaml @@ -26,9 +26,11 @@ jobs: # Steps represent a sequence of tasks that will be executed as part of the job steps: + - uses: actions/checkout@v4 - uses: ros-tooling/setup-ros@v0.7 with: use-ros2-testing: true - uses: ros-tooling/action-ros-ci@v0.3 with: target-ros2-distro: rolling + vcs-repo-file-url: dependencies.repos diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c18dd8d --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__/ diff --git a/dependencies.repos b/dependencies.repos new file mode 100644 index 0000000..9a85c43 --- /dev/null +++ b/dependencies.repos @@ -0,0 +1,5 @@ +repositories: + game_controller_spl: + type: git + url: https://github.com/ros-sports/game_controller_spl.git + version: rolling diff --git a/r2r_spl/package.xml b/r2r_spl/package.xml new file mode 100644 index 0000000..5ccca5e --- /dev/null +++ b/r2r_spl/package.xml @@ -0,0 +1,25 @@ + + + + r2r_spl + 0.0.0 + Robot-to-Robot Communication in RoboCup Standard Platform League + ijnek + Apache License 2.0 + + python3-construct + rclpy + rosidl_parser + game_controller_spl_interfaces + + ament_copyright + ament_flake8 + ament_pep257 + python3-numpy + python3-pytest + r2r_spl_test_interfaces + + + ament_python + + diff --git a/r2r_spl/r2r_spl/__init__.py b/r2r_spl/r2r_spl/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/r2r_spl/r2r_spl/exceptions.py b/r2r_spl/r2r_spl/exceptions.py new file mode 100644 index 0000000..14b09c2 --- /dev/null +++ b/r2r_spl/r2r_spl/exceptions.py @@ -0,0 +1,19 @@ +# Copyright 2023 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. + +class ParameterNotSetException(Exception): + + def __init__(self, message): + self.message = message + super().__init__(message) diff --git a/r2r_spl/r2r_spl/r2r_spl.py b/r2r_spl/r2r_spl/r2r_spl.py new file mode 100644 index 0000000..74af047 --- /dev/null +++ b/r2r_spl/r2r_spl/r2r_spl.py @@ -0,0 +1,179 @@ +# Copyright 2023 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 construct +from game_controller_spl_interfaces.msg import RCGCD15 +from r2r_spl.exceptions import ParameterNotSetException +from r2r_spl.serialization import Serialization +import rclpy +from rclpy.node import Node + +MAX_ALLOWED_MSG_SIZE = 128 + + +class R2RSPL(Node): + """Node that runs on the robot to communicate with teammates (Robot-To-Robot) in SPL.""" + + _loop_thread = None + _sock = None + _publisher = None + _team_num = None + _budget_reached = False + + def __init__(self, node_name='r2r_spl', **kwargs): + super().__init__(node_name, **kwargs) + + # Declare parameters + self.declare_parameters( + namespace='', + parameters=[ + ('team_num', 0), + ('player_num', 0), + ('msg_type', ''), + ('filter_own', False), + ] + ) + + # Read and log parameters + self._team_num = self.get_parameter('team_num').value + self.get_logger().debug('team_num: {}'.format(self._team_num)) + if self._team_num == 0: + self.get_logger().warn('"team_num" parameter is 0. This is problematic in a game.') + + self.player_num = self.get_parameter('player_num').value + self.get_logger().debug('player_num: {}'.format(self.player_num)) + if self.player_num == 0: + self.get_logger().warn('"player_num" parameter is 0. This is problematic in a game.') + + self.msg_type = self.get_parameter('msg_type').value + self.get_logger().debug('msg_type: {}'.format(self.msg_type)) + + self.filter_own = self.get_parameter('filter_own').value + self.get_logger().debug('filter_own: {}'.format(self.filter_own)) + + # Setup subscriber that listens to message budget + self._subscriber_rcgcd = self.create_subscription( + RCGCD15, 'gc/data', self._rcgcd_callback, 10) + + # Evalulate and import message type + if self.msg_type == '': + raise ParameterNotSetException('"msg_type" parameter must be set.') + index_last_dot = self.msg_type.rfind('.') + assert index_last_dot != -1, \ + f'msg_type must be in the form "package_name..MsgName" ' \ + f'(eg. geometry_msgs.msg.PoseStamped). Got: {self.msg_type}' + assert index_last_dot != len(self.msg_type) - 1, \ + f'msg_type must be in the form "package_name..MsgName" ' \ + f'(eg. geometry_msgs.msg.PoseStamped). Got: {self.msg_type}' + class_name = self.msg_type[index_last_dot + 1:] + mod = __import__(self.msg_type[:index_last_dot], fromlist=[class_name]) + msg_class = getattr(mod, class_name) + + # Setup serialization + self._serialization = Serialization( + msg_class, player_num=self.player_num if self.filter_own else None) + + # Setup publisher + self._publisher = self.create_publisher(msg_class, 'r2r/recv', 10) + + # Setup subscriber + self._subscriber = self.create_subscription( + msg_class, 'r2r/send', self._topic_callback, 10) + + # UDP Client - adapted from https://github.com/ninedraft/python-udp/blob/master/client.py + self._sock = 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._sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + self._sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + self._sock.bind(('', 10000 + self._team_num)) + # Set timeout so _loop can constantly check for rclpy.ok() + self._sock.settimeout(0.1) + + # Start thread to continuously poll + self._loop_thread = Thread(target=self._loop) + self._loop_thread.start() + + def _loop(self): + while rclpy.ok(): + try: + data, _ = self._sock.recvfrom(1024) + self.get_logger().debug(f'received: {data}') + + # Convert data to ROS msg + try: + msg = self._serialization.deserialize(data) + + if msg: + # Publish. Make sure we haven't shutdown, since node could have been + # shutdown while waiting for the packet. + if rclpy.ok(): + self._publisher.publish(msg) + else: + # Reaches here if we filtered out our own message + # (only if filtering is enabled via filter_own parameter) + pass + + except construct.core.StreamError: + # Deserialization failed + self.get_logger().error( + f'deserialization failed, please ensure other robots are using the ' + f'matching message type {self.msg_type}', once=True) + + except TimeoutError: + pass + + def _topic_callback(self, msg): + if not self._budget_reached: + data = self._serialization.serialize(msg) + + if len(data) > MAX_ALLOWED_MSG_SIZE: + self.get_logger().error( + f'Cannot send message of size {len(data)} bytes. Maximum size is 128 bytes.') + else: + # Broadcast data on team's UDP port + self._sock.sendto(data, ('', 10000 + self._team_num)) + + def _rcgcd_callback(self, msg): + team_found = False + for team in msg.teams: + if team.team_number == self._team_num: + team_found = True + + if not self._budget_reached and team.message_budget < 10: + self.get_logger().info('Budget almost reached, not sending anymore messages') + self._budget_reached = True + elif self._budget_reached and team.message_budget > 10: + self.get_logger().info('Extra budget available, sending messages again') + self._budget_reached = False + + if not team_found: + self.get_logger().warn( + f'Received messages from Game Controller about teams {msg.teams[0].team_number} ' + f'and {msg.teams[1].team_number}, but team_num parameter is {self._team_num}. ' + f'This is problematic if in a game.', once=True) + + +def main(args=None): + rclpy.init(args=args) + r2r_spl = R2RSPL() + rclpy.spin(r2r_spl) + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/r2r_spl/r2r_spl/serialization.py b/r2r_spl/r2r_spl/serialization.py new file mode 100644 index 0000000..56797e4 --- /dev/null +++ b/r2r_spl/r2r_spl/serialization.py @@ -0,0 +1,217 @@ +# Copyright 2023 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 construct +import rosidl_parser.definition + + +class Serialization: + + def __init__(self, msg_class, player_num=None): + """Set player_num if you want to filter out messages sent by yourself.""" + # Store msg_class and player number + self.msg_class = msg_class + self.player_num = player_num + + # Array to store struct members + members = [] + + # Start struct with player_num, if specified + if player_num is not None: + members.append('player_num' / construct.Int8ul) + + # Add message content + members.append('content' / to_struct(msg_class)) + + # Create struct + self.struct = construct.Struct(*members) + + def serialize(self, msg_instance): + """Serialize a message to a byte array.""" + # Map to store values + values = {} + + # Set player_num, if specified + if self.player_num is not None: + values['player_num'] = self.player_num + + # Set message content + values['content'] = to_container(msg_instance) + + # Create container from map + container = construct.Container(**values) + + # Build container, and return + return self.struct.build(container) + + def deserialize(self, serialized): + """ + Deserialize a byte array to a ROS message. + + Returns None, if message received was one sent from ourself. + Raises construct.core.StreamError if deserialization fails. + """ + parsed = self.struct.parse(serialized) + + # If player_num is specified, check if it matches the message's player_num + # If it does, return None + if self.player_num is not None: + if self.player_num == parsed['player_num']: + return None + + return to_msg(parsed['content'], self.msg_class) + + +basic_type_conversion = { + 'float': construct.Float32l, + 'double': construct.Float64l, + 'boolean': construct.Flag, + 'octet': construct.Bytes(1), + 'int8': construct.Int8sl, + 'uint8': construct.Int8ul, + 'int16': construct.Int16sl, + 'uint16': construct.Int16ul, + 'int32': construct.Int32sl, + 'uint32': construct.Int32ul, + 'int64': construct.Int64sl, + 'uint64': construct.Int64ul, +} + + +def to_struct(msg_class) -> construct.Struct: + """Convert a message to a construct struct.""" + members = [] + for s, t in zip(msg_class.get_fields_and_field_types().keys(), msg_class.SLOT_TYPES): + # Nested Type + if isinstance(t, rosidl_parser.definition.NamespacedType): + mod = __import__('.'.join(t.namespaces), fromlist=[t.name]) + klass = getattr(mod, t.name) + members.append(s / to_struct(klass)) + + # Array + elif isinstance(t, rosidl_parser.definition.Array): + if isinstance(t.value_type, rosidl_parser.definition.NamespacedType): + mod = __import__('.'.join(t.value_type.namespaces), fromlist=[t.value_type.name]) + klass = getattr(mod, t.value_type.name) + tmp_type = to_struct(klass) + elif isinstance(t.value_type, rosidl_parser.definition.BasicType): + tmp_type = basic_type_conversion[t.value_type.typename] + else: + tmp_type = to_struct(t.value_type) + members.append(s / construct.Array(t.size, tmp_type)) + + # Unbounded sequence + # Bounded sequence + elif (isinstance(t, rosidl_parser.definition.UnboundedSequence) or + isinstance(t, rosidl_parser.definition.BoundedSequence)): + if isinstance(t.value_type, rosidl_parser.definition.NamespacedType): + mod = __import__('.'.join(t.value_type.namespaces), fromlist=[t.value_type.name]) + klass = getattr(mod, t.value_type.name) + tmp_type = to_struct(klass) + elif isinstance(t.value_type, rosidl_parser.definition.BasicType): + tmp_type = basic_type_conversion[t.value_type.typename] + else: + tmp_type = to_struct(t.value_type) + members.append(s / construct.PrefixedArray(construct.VarInt, tmp_type)) + + # Unbounded string + # Bounded string + elif (isinstance(t, rosidl_parser.definition.UnboundedString) or + isinstance(t, rosidl_parser.definition.BoundedString)): + members.append(s / construct.PascalString(construct.VarInt, 'utf8')) + + # Unbounded wstring + # Bounded wstring + elif (isinstance(t, rosidl_parser.definition.UnboundedWString) or + isinstance(t, rosidl_parser.definition.BoundedWString)): + members.append(s / construct.PascalString(construct.VarInt, 'utf16')) + + # Basic type + elif isinstance(t, rosidl_parser.definition.BasicType): + members.append(s / basic_type_conversion[t.typename]) + return construct.Struct(*members) + + +def to_container(msg_instance) -> construct.Container: + values = {} + for s, t in zip(msg_instance.get_fields_and_field_types().keys(), msg_instance.SLOT_TYPES): + # Check if namespaced type + if isinstance(t, rosidl_parser.definition.NamespacedType): + field = getattr(msg_instance, s) + values[s] = to_container(field) + # Check if array type + elif (isinstance(t, rosidl_parser.definition.Array) or + isinstance(t, rosidl_parser.definition.UnboundedSequence) or + isinstance(t, rosidl_parser.definition.BoundedSequence)): + field = getattr(msg_instance, s) + if isinstance(t.value_type, rosidl_parser.definition.NamespacedType): + values[s] = [to_container(f) for f in field] + elif isinstance(t.value_type, rosidl_parser.definition.BasicType): + if t.value_type.typename in rosidl_parser.definition.INTEGER_TYPES or \ + t.value_type.typename in rosidl_parser.definition.FLOATING_POINT_TYPES: + values[s] = field.tolist() + else: + values[s] = field + # Check if string type + elif (isinstance(t, rosidl_parser.definition.UnboundedString) or + isinstance(t, rosidl_parser.definition.BoundedString)): + values[s] = getattr(msg_instance, s) + # Check if wstring type + elif (isinstance(t, rosidl_parser.definition.UnboundedWString) or + isinstance(t, rosidl_parser.definition.BoundedWString)): + values[s] = getattr(msg_instance, s) + # Check if basic type + elif isinstance(t, rosidl_parser.definition.BasicType): + values[s] = getattr(msg_instance, s) + return construct.Container(**values) + + +def to_msg(container, msg_class): + values = {} + + for s, t in zip(msg_class.get_fields_and_field_types().keys(), msg_class.SLOT_TYPES): + if s in container: + # Nested Type + if isinstance(t, rosidl_parser.definition.NamespacedType): + mod = __import__('.'.join(t.namespaces), fromlist=[t.name]) + klass = getattr(mod, t.name) + values[s] = to_msg(container[s], klass) + # Array + elif (isinstance(t, rosidl_parser.definition.Array) or + isinstance(t, rosidl_parser.definition.UnboundedSequence) or + isinstance(t, rosidl_parser.definition.BoundedSequence)): + tmp_array = [] + if isinstance(t.value_type, rosidl_parser.definition.NamespacedType): + mod = __import__('.'.join(t.value_type.namespaces), + fromlist=[t.value_type.name]) + klass = getattr(mod, t.value_type.name) + for v in container[s]: + tmp_array.append(to_msg(v, klass)) + else: + for v in container[s]: + tmp_array.append(v) + values[s] = tmp_array + # Basic Type + # Unbounded string + # Bounded string + # Unbounded wstring + # Bounded wstring + elif (isinstance(t, rosidl_parser.definition.BasicType) or + isinstance(t, rosidl_parser.definition.UnboundedString) or + isinstance(t, rosidl_parser.definition.BoundedString) or + isinstance(t, rosidl_parser.definition.UnboundedWString) or + isinstance(t, rosidl_parser.definition.BoundedWString)): + values[s] = container[s] + + return msg_class(**values) diff --git a/r2r_spl/resource/r2r_spl b/r2r_spl/resource/r2r_spl new file mode 100644 index 0000000..e69de29 diff --git a/r2r_spl/setup.cfg b/r2r_spl/setup.cfg new file mode 100644 index 0000000..64ab8c0 --- /dev/null +++ b/r2r_spl/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/r2r_spl +[install] +install_scripts=$base/lib/r2r_spl diff --git a/r2r_spl/setup.py b/r2r_spl/setup.py new file mode 100644 index 0000000..7f2b603 --- /dev/null +++ b/r2r_spl/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'r2r_spl' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ijnek', + maintainer_email='kenjibrameld@gmail.com', + description='Robot-to-Robot Communication in RoboCup Standard Platform League', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'r2r_spl = r2r_spl.r2r_spl:main', + ], + }, +) diff --git a/r2r_spl/test/test_copyright.py b/r2r_spl/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/r2r_spl/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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 ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/r2r_spl/test/test_flake8.py b/r2r_spl/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/r2r_spl/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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 ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/r2r_spl/test/test_pep257.py b/r2r_spl/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/r2r_spl/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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 ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/r2r_spl/test/test_r2r_spl.py b/r2r_spl/test/test_r2r_spl.py new file mode 100644 index 0000000..27c53a6 --- /dev/null +++ b/r2r_spl/test/test_r2r_spl.py @@ -0,0 +1,350 @@ +# Copyright 2023 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 +import time +import unittest + +from game_controller_spl_interfaces.msg import RCGCD15 +from r2r_spl.r2r_spl import R2RSPL +from r2r_spl.serialization import Serialization +from r2r_spl_test_interfaces.msg import ArrayTypes, BasicTypes +import rclpy +from rclpy.parameter import Parameter + + +class TestR2RSPL(unittest.TestCase): + """Tests against R2RSPL.""" + + received = None + + team_num = 1 + player_num = 1 + + parameter_overrides = [ + Parameter('team_num', value=team_num), + Parameter('player_num', value=player_num), + Parameter('msg_type', value='r2r_spl_test_interfaces.msg.ArrayTypes')] + + def _callback_msg(self, msg): + self.received = msg + + def test_receiving(self): + """Test receiving UDP package from teammate.""" + rclpy.init() + + # Setup nodes + r2r_spl_node = R2RSPL(parameter_overrides=self.parameter_overrides) # noqa: F841 + test_node = rclpy.node.Node('test') + subscription = test_node.create_subscription( # noqa: F841 + ArrayTypes, 'r2r/recv', self._callback_msg, 10) + + # Example message from another player on same team + serialization = Serialization(ArrayTypes) + serialized = serialization.serialize(ArrayTypes()) + + # UDP - adapted from https://github.com/ninedraft/python-udp/blob/8/server.py + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) as sock: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + sock.sendto(serialized, ('', 10000 + self.team_num)) + + # Wait for R2RSPL to receive packet over UDP, and publish a ROS msg + time.sleep(0.1) + + # Check if message has been received on topic + rclpy.spin_once(test_node, timeout_sec=0) + self.assertIsNotNone(self.received) + + # Shutdown, then ensure thread is joined + rclpy.shutdown() + r2r_spl_node._loop_thread.join() + + def test_filter_own(self): + """ + Test filtering of packages, when filter_own param is set to True. + + Check: + - Packet sent by myself is ignored + - Packet from teammate is processed + """ + rclpy.init() + + r2r_spl_node = R2RSPL(parameter_overrides=[ # noqa: F841 + Parameter('team_num', value=self.team_num), + Parameter('player_num', value=self.player_num), + Parameter('msg_type', value='r2r_spl_test_interfaces.msg.ArrayTypes'), + Parameter('filter_own', value=True)]) + test_node = rclpy.node.Node('test') + subscription = test_node.create_subscription( # noqa: F841 + ArrayTypes, 'r2r/recv', self._callback_msg, 10) + + # Send message from myself + serialization = Serialization(ArrayTypes, player_num=self.player_num) + serialized = serialization.serialize(ArrayTypes()) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) as sock: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + sock.sendto(serialized, ('', 10000 + self.team_num)) + + # Wait for R2RSPL to receive packet over UDP + time.sleep(0.1) + + # Check if message has been received on topic + rclpy.spin_once(test_node, timeout_sec=0) + + # Expect no message published + self.assertIsNone(self.received) + + # Send message from another player on same team + serialization = Serialization(ArrayTypes, player_num=5) + serialized = serialization.serialize(ArrayTypes()) + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) as sock: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + sock.sendto(serialized, ('', 10000 + self.team_num)) + + # Wait for R2RSPL to receive packet over UDP + time.sleep(0.1) + + # Check if message has been received on topic + rclpy.spin_once(test_node, timeout_sec=0) + + # Expect message to be published + self.assertIsNotNone(self.received) + + # Shutdown, then ensure thread is joined + rclpy.shutdown() + r2r_spl_node._loop_thread.join() + + def test_sending(self): + """Test sending UDP package to teammate.""" + rclpy.init() + + r2r_spl_node = R2RSPL(parameter_overrides=self.parameter_overrides) # noqa: F841 + test_node = rclpy.node.Node('test') + publisher = test_node.create_publisher(ArrayTypes, 'r2r/send', 10) + + # UDP - adapted from https://github.com/ninedraft/python-udp/blob/8/server.py + sock = socket.socket( + socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + sock.bind(('', 10000 + self.team_num)) + sock.settimeout(0.1) + + # Publish ArrayTypes to r2r_spl_node + publisher.publish(ArrayTypes()) + + # Wait before spinning for the msg arrive in r2r_spl_node's subscription + time.sleep(0.1) + + # Spin r2r_spl_node to process incoming message and send out UDP message + rclpy.spin_once(r2r_spl_node, timeout_sec=0) + + # Check if packet has arrived + try: + _ = sock.recv(1024) + except TimeoutError: + self.fail('TimeoutError, did not receive expected UDP packet') + + # Close socket + sock.close() + + # Shutdown, then ensure thread is joined + rclpy.shutdown() + r2r_spl_node._loop_thread.join() + + def test_invalid_msg_type(self): + """ + Test msg type parameter that is in the wrong format. + + Check: + - Msg type not containing a dot (eg. r2r_spl_test_interfaces/msg/ArrayTypes) + - Msg type ending with a dot (eg. r2r_spl_test_interfaces.msg.) + """ + rclpy.init() + + with self.assertRaises(AssertionError): + R2RSPL(parameter_overrides=[ + Parameter('msg_type', value='r2r_spl_test_interfaces/msg/ArrayTypes')]) + with self.assertRaises(AssertionError): + R2RSPL(parameter_overrides=[ + Parameter('msg_type', value='r2r_spl_test_interfaces.msg.')]) + rclpy.shutdown() + + def test_msg_type_not_found(self): + """Test msg type parameter with non-existent message type.""" + rclpy.init() + + with self.assertRaises(ModuleNotFoundError): + R2RSPL(parameter_overrides=[ + Parameter('msg_type', value='NonExistentPackage.msg.ArrayTypes')]) + with self.assertRaises(AttributeError): + R2RSPL(parameter_overrides=[ + Parameter('msg_type', value='r2r_spl_test_interfaces.msg.NonExistentType')]) + rclpy.shutdown() + + def test_wrong_packet_size(self): + """Test receiving UDP package of incorrect size (incompatible message type).""" + rclpy.init() + + # Setup nodes + r2r_spl_node = R2RSPL(parameter_overrides=self.parameter_overrides) # noqa: F841 + test_node = rclpy.node.Node('test') + + # Incorrect message type from another player on same team + serialization = Serialization(BasicTypes) + serialized = serialization.serialize(BasicTypes()) + + # UDP - adapted from https://github.com/ninedraft/python-udp/blob/8/server.py + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) as sock: + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + sock.sendto(serialized, ('', 10000 + self.team_num)) + + # Wait for R2RSPL to receive packet over UDP + time.sleep(0.1) + + # Check if message has been received on topic + rclpy.spin_once(test_node, timeout_sec=0) + + # Expect no message published + self.assertIsNone(self.received) + + # Shutdown, then ensure thread is joined + rclpy.shutdown() + r2r_spl_node._loop_thread.join() + + def test_message_budget(self): + """ + Test to ensure the SPL message budget is not exceeded. + + Check: + - Sending stops when message budget is low + - Sending restarts when extra budget is added + """ + rclpy.init() + + r2r_spl_node = R2RSPL(parameter_overrides=self.parameter_overrides) # noqa: F841 + test_node = rclpy.node.Node('test') + publisher = test_node.create_publisher(ArrayTypes, 'r2r/send', 10) + publisher_rcgcd = test_node.create_publisher(RCGCD15, 'gc/data', 10) + + # Publish RCGCD with low (<10) message budget + rcgcd_msg = RCGCD15() + rcgcd_msg.teams[0].team_number = self.team_num + rcgcd_msg.teams[0].message_budget = 5 + publisher_rcgcd.publish(rcgcd_msg) + + # Wait before spinning for the msg arrive in r2r_spl_node's subscription + time.sleep(0.1) + + # Spin r2r_spl_node to process incoming message + rclpy.spin_once(r2r_spl_node, timeout_sec=0) + + # UDP - adapted from https://github.com/ninedraft/python-udp/blob/8/server.py + sock = socket.socket( + socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + sock.bind(('', 10000 + self.team_num)) + sock.settimeout(0.1) + + # Publish ArrayTypes to r2r_spl_node + publisher.publish(ArrayTypes()) + + # Wait before spinning for the msg arrive in r2r_spl_node's subscription + time.sleep(0.1) + + # Spin r2r_spl_node to process incoming message and send out UDP message + rclpy.spin_once(r2r_spl_node, timeout_sec=0) + + # Check to see that packet didn't arrive + with self.assertRaises(TimeoutError): + _ = sock.recv(1024) + + # Publish RCGCD with increased message budget (simulating extra time rule) + rcgcd_msg.teams[0].message_budget = 60 + publisher_rcgcd.publish(rcgcd_msg) + + # Wait before spinning for the msg arrive in r2r_spl_node's subscription + time.sleep(0.1) + + # Spin r2r_spl_node to process incoming message + rclpy.spin_once(r2r_spl_node, timeout_sec=0) + + # Publish ArrayTypes to r2r_spl_node + publisher.publish(ArrayTypes()) + + # Wait before spinning for the msg arrive in r2r_spl_node's subscription + time.sleep(0.1) + + # Spin r2r_spl_node to process incoming message and send out UDP message + rclpy.spin_once(r2r_spl_node, timeout_sec=0) + + # Check if packet has arrived + try: + _ = sock.recv(1024) + except TimeoutError: + self.fail('TimeoutError, did not receive expected UDP packet') + + # Close socket + sock.close() + + # Shutdown, then ensure thread is joined + rclpy.shutdown() + r2r_spl_node._loop_thread.join() + + def test_msg_size_exceeding_128_bytes(self): + """Test to ensure we don't send messages exceeding 128 bytes.""" + rclpy.init() + r2r_spl_node = R2RSPL(parameter_overrides=self.parameter_overrides) # noqa: F841 + test_node = rclpy.node.Node('test') + publisher = test_node.create_publisher(ArrayTypes, 'r2r/send', 10) + + # UDP - adapted from https://github.com/ninedraft/python-udp/blob/8/server.py + sock = socket.socket( + socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) + sock.bind(('', 10000 + self.team_num)) + sock.settimeout(0.1) + + # Publish ArrayTypes to r2r_spl_node + publisher.publish(ArrayTypes(data_string='a' * 129)) + + # Wait before spinning for the msg arrive in r2r_spl_node's subscription + time.sleep(0.1) + + # Spin r2r_spl_node to process incoming message and send out UDP message + rclpy.spin_once(r2r_spl_node, timeout_sec=0) + + # Check to see that packet didn't arrive + with self.assertRaises(TimeoutError): + _ = sock.recv(1024) + + # Expect no message published + self.assertIsNone(self.received) + + # Close socket + sock.close() + + # Shutdown, then ensure thread is joined + rclpy.shutdown() + r2r_spl_node._loop_thread.join() + + +if __name__ == '__main__': + unittest.main() diff --git a/r2r_spl/test/test_serialization.py b/r2r_spl/test/test_serialization.py new file mode 100755 index 0000000..e91398c --- /dev/null +++ b/r2r_spl/test/test_serialization.py @@ -0,0 +1,137 @@ +# Copyright 2023 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 unittest + +import numpy.testing + +from r2r_spl.serialization import Serialization +from r2r_spl_test_interfaces.msg import ArrayTypes, BasicTypes, NestedTypes + + +class TestSerialization(unittest.TestCase): + + def test_basic_types(self): + """Test serialization and deserialization of basic types.""" + # Initialize serialization object + serialization = Serialization(BasicTypes) + + # Create message instance + msg_instance = BasicTypes() + msg_instance.val_bool = True + msg_instance.val_byte = b'\x10' + msg_instance.val_char = ord('A') # char comes in as a uint8 + msg_instance.val_float32 = 0.01 + msg_instance.val_float64 = 0.02 + msg_instance.val_int8 = -1 + msg_instance.val_uint8 = 2 + msg_instance.val_int16 = -3 + msg_instance.val_uint16 = 4 + msg_instance.val_int32 = -5 + msg_instance.val_uint32 = 6 + msg_instance.val_int64 = -7 + msg_instance.val_uint64 = 8 + + # Serialize + serialized = serialization.serialize(msg_instance) + + # Deserialize + deserialized = serialization.deserialize(serialized) + + # Check type matches original message + self.assertEqual(type(msg_instance), type(deserialized)) + + # Check values match original message + self.assertEqual(deserialized.val_bool, True) + self.assertEqual(chr(deserialized.val_char), 'A') # char comes out as a uint8 + self.assertEqual(deserialized.val_byte, b'\x10') + self.assertAlmostEqual(deserialized.val_float32, 0.01) + self.assertAlmostEqual(deserialized.val_float64, 0.02) + self.assertEqual(deserialized.val_int8, -1) + self.assertEqual(deserialized.val_uint8, 2) + self.assertEqual(deserialized.val_int16, -3) + self.assertEqual(deserialized.val_uint16, 4) + self.assertEqual(deserialized.val_int32, -5) + self.assertEqual(deserialized.val_uint32, 6) + self.assertEqual(deserialized.val_int64, -7) + self.assertEqual(deserialized.val_uint64, 8) + + def test_array_types(self): + """Test serialization and deserialization of array types.""" + # Initialize serialization object + serialization = Serialization(ArrayTypes) + + # Create message instance + msg_instance = ArrayTypes() + msg_instance.data_int8_static = [1, 2, 3] + msg_instance.data_bool_static = [False, True] + msg_instance.data_int8_unbounded_dynamic = [10, 20, 30, 40] + msg_instance.data_bool_unbounded_dynamic = [True, False, True, False, True] + msg_instance.data_int8_bounded_dynamic = [70, 80] + msg_instance.data_bool_bounded_dynamic = [False] + msg_instance.data_string = 'Hello World' + msg_instance.data_string_bounded = 'Hi' + msg_instance.data_wstring = 'ハローワールド' + msg_instance.data_wstring_bounded = 'やあ' + + # Serialize + serialized = serialization.serialize(msg_instance) + + # Deserialize + deserialized = serialization.deserialize(serialized) + + # Check type matches original message + self.assertEqual(type(msg_instance), type(deserialized)) + + # Check values match original message + numpy.testing.assert_array_equal(deserialized.data_int8_static, [1, 2, 3]) + self.assertEqual(deserialized.data_bool_static, [False, True]) + numpy.testing.assert_array_equal( + deserialized.data_int8_unbounded_dynamic, [10, 20, 30, 40]) + self.assertEqual( + deserialized.data_bool_unbounded_dynamic, [True, False, True, False, True]) + numpy.testing.assert_array_equal(deserialized.data_int8_bounded_dynamic, [70, 80]) + self.assertEqual(deserialized.data_bool_bounded_dynamic, [False]) + self.assertEqual(deserialized.data_string, 'Hello World') + self.assertEqual(deserialized.data_string_bounded, 'Hi') + self.assertEqual(deserialized.data_wstring, 'ハローワールド') + self.assertEqual(deserialized.data_wstring_bounded, 'やあ') + + def test_nested_types(self): + """Test serialization and deserialization of nested types.""" + # Initialize serialization object + serialization = Serialization(NestedTypes) + + # Create message instance + msg_instance = NestedTypes() + msg_instance.data_basic_types.val_uint8 = 101 + msg_instance.data_basic_types_array = [BasicTypes(), BasicTypes(val_int8=-10)] + + # Serialize + serialized = serialization.serialize(msg_instance) + + # Deserialize + deserialized = serialization.deserialize(serialized) + + # Check type matches original message + self.assertEqual(type(msg_instance), type(deserialized)) + + # Check values match original message + self.assertEqual(deserialized.data_basic_types.val_uint8, 101) + self.assertEqual(len(deserialized.data_basic_types_array), 2) + self.assertEqual(deserialized.data_basic_types_array[1].val_int8, -10) + + +if __name__ == '__main__': + unittest.main() diff --git a/r2r_spl_7/test/test_r2r_spl.py b/r2r_spl_7/test/test_r2r_spl.py index 444dff8..19fa544 100644 --- a/r2r_spl_7/test/test_r2r_spl.py +++ b/r2r_spl_7/test/test_r2r_spl.py @@ -32,7 +32,7 @@ class TestR2RSPL: received = None - team_num = 1 + team_num = 2 player_num = 1 parameter_overrides = [ diff --git a/r2r_spl_8/test/test_r2r_spl.py b/r2r_spl_8/test/test_r2r_spl.py index 624e0f5..1b2814d 100644 --- a/r2r_spl_8/test/test_r2r_spl.py +++ b/r2r_spl_8/test/test_r2r_spl.py @@ -32,7 +32,7 @@ class TestR2RSPL: received = None - team_num = 1 + team_num = 3 player_num = 1 parameter_overrides = [ diff --git a/r2r_spl_test_interfaces/CMakeLists.txt b/r2r_spl_test_interfaces/CMakeLists.txt new file mode 100644 index 0000000..f1a8c11 --- /dev/null +++ b/r2r_spl_test_interfaces/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) +project(r2r_spl_test_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# generate interfaces +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/ArrayTypes.msg" + "msg/BasicTypes.msg" + "msg/NestedTypes.msg" +) + +# export dependencies +ament_export_dependencies(rosidl_default_runtime) + +# test +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/r2r_spl_test_interfaces/msg/ArrayTypes.msg b/r2r_spl_test_interfaces/msg/ArrayTypes.msg new file mode 100644 index 0000000..17d2526 --- /dev/null +++ b/r2r_spl_test_interfaces/msg/ArrayTypes.msg @@ -0,0 +1,10 @@ +int8[3] data_int8_static # Test numeric static array (since it uses numpy arrays) +bool[2] data_bool_static # Test non-numeric static array +int8[] data_int8_unbounded_dynamic # Test numeric unbounded array +bool[] data_bool_unbounded_dynamic # Test non-numeric unbounded array +int8[<=3] data_int8_bounded_dynamic # Test numeric bounded array +bool[<=2] data_bool_bounded_dynamic # Test non-numeric bounded array +string data_string # Test string +string<=3 data_string_bounded # Test bounded string +wstring data_wstring # Test wstring +wstring<=3 data_wstring_bounded # Test bounded wstring diff --git a/r2r_spl_test_interfaces/msg/BasicTypes.msg b/r2r_spl_test_interfaces/msg/BasicTypes.msg new file mode 100644 index 0000000..f948ab6 --- /dev/null +++ b/r2r_spl_test_interfaces/msg/BasicTypes.msg @@ -0,0 +1,13 @@ +bool val_bool +byte val_byte +char val_char +float32 val_float32 +float64 val_float64 +int8 val_int8 +uint8 val_uint8 +int16 val_int16 +uint16 val_uint16 +int32 val_int32 +uint32 val_uint32 +int64 val_int64 +uint64 val_uint64 diff --git a/r2r_spl_test_interfaces/msg/NestedTypes.msg b/r2r_spl_test_interfaces/msg/NestedTypes.msg new file mode 100644 index 0000000..b09a958 --- /dev/null +++ b/r2r_spl_test_interfaces/msg/NestedTypes.msg @@ -0,0 +1,4 @@ +# Message containing other message types + +r2r_spl_test_interfaces/BasicTypes data_basic_types +r2r_spl_test_interfaces/BasicTypes[] data_basic_types_array diff --git a/r2r_spl_test_interfaces/package.xml b/r2r_spl_test_interfaces/package.xml new file mode 100644 index 0000000..3af6844 --- /dev/null +++ b/r2r_spl_test_interfaces/package.xml @@ -0,0 +1,22 @@ + + + + r2r_spl_test_interfaces + 0.0.0 + Messages for testing r2r_spl + ijnek + Apache License 2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + +