Add r2r_spl package, capable of listening to different msg types

Signed-off-by: Kenji Brameld <kenjibrameld@gmail.com>

.gitignore

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

README.md

@@ -3,6 +3,7 @@
 [![Build and Test (humble)](../../actions/workflows/build_and_test_humble.yaml/badge.svg?branch=humble)](../../actions/workflows/build_and_test_humble.yaml?query=branch:humble)
 [![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)
+[![Build and Test (dev)](../../actions/workflows/build_and_test_dev.yaml/badge.svg?branch=dev)](../../actions/workflows/build_and_test_dev.yaml?query=branch:dev)
 
 ROS2 package that handles intra-team communication using SPLStandardMessage defined in the RoboCup SPL rulebook.
 

dependencies.repos

@@ -0,0 +1,5 @@
+repositories:
+  gc_spl:
+    type: git
+    url: https://github.com/ros-sports/gc_spl.git
+    version: rolling

r2r_spl/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>r2r_spl</name>
+  <version>0.0.0</version>
+  <description>Robot-to-Robot Communication in RoboCup Standard Platform League</description>
+  <maintainer email="kenjibrameld@gmail.com">vscode</maintainer>
+  <license>Apache License 2.0</license>
+
+  <exec_depend>python3-construct</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>rosidl_parser</exec_depend>
+  <exec_depend>gc_spl_interfaces</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-numpy</test_depend>
+  <test_depend>python3-pytest</test_depend>
+  <test_depend>r2r_spl_test_interfaces</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

r2r_spl/r2r_spl/r2r_spl.py

@@ -0,0 +1,170 @@
+# 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 gc_spl_interfaces.msg import RCGCD15
+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))
+
+        self.player_num = self.get_parameter('player_num').value
+        self.get_logger().debug('player_num: {}'.format(self.player_num))
+
+        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
+        index_last_dot = self.msg_type.rfind('.')
+        assert index_last_dot != -1, \
+            f'msg_type must be in the form "package_name.<namespace>.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.<namespace>.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
+                        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()

r2r_spl/r2r_spl/serialization.py

@@ -0,0 +1,177 @@
+# 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 self.msg_class(**parsed['content'])
+
+
+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)

r2r_spl/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/r2r_spl
+[install]
+install_scripts=$base/lib/r2r_spl