Multi robot sim fixes

src/bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py

@@ -61,8 +61,8 @@ def generate_domain_bridge_config(robot_domain: int, output_dir: Path) -> Path:
     sensor_topics = [
         ("joint_states", "sensor_msgs/msg/JointState"),
         ("imu/data", "sensor_msgs/msg/Imu"),
-        ("camera/image_proc", "sensor_msgs/msg/Image"),
-        ("camera/camera_info", "sensor_msgs/msg/CameraInfo"),
+        ("zed/zed_node/rgb/image_rect_color", "sensor_msgs/msg/Image"),
+        ("zed/zed_node/rgb/camera_info", "sensor_msgs/msg/CameraInfo"),
     ]
 
     for topic_suffix, msg_type in sensor_topics:
@@ -84,6 +84,29 @@ def generate_domain_bridge_config(robot_domain: int, output_dir: Path) -> Path:
     config_path = output_dir / f"robot{robot_domain}_bridge.yaml"
     with open(config_path, "w") as f:
         yaml.dump(config, f, default_flow_style=False, sort_keys=False)
+        # Team communication: forwarded one-way in each direction, on two distinct topic names.
+        # Every robot's bridge mirrors its own "team_comm_binary_transport/out" into the shared
+        # main domain (where all robots' publishers merge into one topic), then relays that
+        # merged topic back out to "team_comm_binary_transport/in" in its own domain.
+        # A single `bidirectional: true` bridge of one shared topic name was tried instead and
+        # causes an unbounded forwarding loop once 3+ domains are bridged through the same hub
+        # domain (confirmed experimentally) - see RosCommunication in
+        # bitbots_team_communication/communication.py for the full explanation. Two distinct
+        # topic names make that loop structurally impossible.
+        # Written as raw YAML appended to the "topics" map (instead of via the `config` dict)
+        # because both entries need the literal key "team_comm_binary_transport/out", which a
+        # Python dict can't hold twice.
+        f.write(
+            "  team_comm_binary_transport/out:\n"
+            "    type: std_msgs/msg/UInt8MultiArray\n"
+            f"    from_domain: {robot_domain}\n"
+            f"    to_domain: {main_domain}\n"
+            "  team_comm_binary_transport/out:\n"
+            "    type: std_msgs/msg/UInt8MultiArray\n"
+            f"    from_domain: {main_domain}\n"
+            f"    to_domain: {robot_domain}\n"
+            "    remap: team_comm_binary_transport/in\n"
+        )
 
     return config_path
 

src/bitbots_misc/bitbots_bringup/package.xml

@@ -28,6 +28,7 @@
     <exec_depend>bitbots_robot_description</exec_depend>
     <exec_depend>bitbots_utils</exec_depend>
     <exec_depend>bitbots_vision</exec_depend>
+    <exec_depend>domain_bridge</exec_depend>
     <exec_depend>foxglove_bridge</exec_depend>
     <exec_depend>game_controller_hsl</exec_depend>
     <exec_depend>humanoid_base_footprint</exec_depend>

src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py

@@ -1,10 +1,9 @@
 #!/usr/bin/env python3
 
-import socket
-import struct
 import threading
 from typing import Optional
 
+import numpy as np
 import rclpy
 import transforms3d
 from ament_index_python.packages import get_package_share_directory
@@ -13,7 +12,7 @@
 from builtin_interfaces.msg import Time as TimeMsg
 from game_controller_hsl_interfaces.msg import GameState, PlayerStatusPose
 from geometry_msgs.msg import PoseWithCovarianceStamped, Quaternion, Twist, TwistWithCovarianceStamped
-from numpy import double
+from jaxtyping import Float64
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.duration import Duration
 from rclpy.experimental.events_executor import EventsExecutor
@@ -26,7 +25,7 @@
 
 import bitbots_team_communication.robocup_extension_pb2 as Proto  # noqa: N812
 from bitbots_msgs.msg import Strategy, TeamData
-from bitbots_team_communication.communication import SocketCommunication
+from bitbots_team_communication.communication import CommunicationBackend, RosCommunication, SocketCommunication
 from bitbots_team_communication.converter.robocup_protocol_converter import RobocupProtocolConverter, TeamColor
 
 
@@ -47,7 +46,12 @@ def __init__(self):
         self.protocol_converter = RobocupProtocolConverter(TeamColor(self.team_color_id))
 
         self.logger.info(f"Starting for {self.player_id} in team {self.team_id}...")
-        self.socket_communication = SocketCommunication(self.node, self.logger, self.team_id, self.player_id)
+        transport: str = self.node.get_parameter("transport").value
+        self.communication: CommunicationBackend = (
+            RosCommunication(self.node, self.logger)
+            if transport == "ros_topic"
+            else SocketCommunication(self.node, self.logger, self.team_id, self.player_id)
+        )
 
         self.rate: int = self.node.get_parameter("rate").value
         self.lifetime: int = self.node.get_parameter("lifetime").value
@@ -67,7 +71,8 @@ def __init__(self):
         self.try_to_establish_connection()
 
         self.node.create_timer(1 / self.rate, self.send_message, callback_group=MutuallyExclusiveCallbackGroup())
-        self.receive_forever()
+        self.communication.start_receiving(self.handle_message)
+        self.block_until_shutdown()
 
     def spin(self):
         executor = EventsExecutor()
@@ -89,7 +94,7 @@ def set_state_defaults(self):
         self.cmd_vel_time = Time(clock_type=self.node.get_clock().clock_type)
         self.ball: Optional[PointStamped] = None
         self.ball_velocity: tuple[float, float, float] = (0.0, 0.0, 0.0)
-        self.ball_covariance: list[double] = []
+        self.ball_covariance: Float64[np.ndarray, "36"] = np.zeros(36, dtype=np.float64)
         self.strategy: Optional[Strategy] = None
         self.strategy_time = Time(clock_type=self.node.get_clock().clock_type)
         self.time_to_ball: Optional[float] = None
@@ -99,8 +104,12 @@ def set_state_defaults(self):
 
     def try_to_establish_connection(self):
         # we will try multiple times till we manage to get a connection
-        while rclpy.ok() and not self.socket_communication.is_setup():
-            self.socket_communication.establish_connection()
+        while rclpy.ok() and not self.communication.is_setup():
+            self.communication.establish_connection()
+            self.node.get_clock().sleep_for(Duration(seconds=1))
+
+    def block_until_shutdown(self):
+        while rclpy.ok():
             self.node.get_clock().sleep_for(Duration(seconds=1))
 
     def create_publishers(self):
@@ -224,7 +233,7 @@ def ball_cb(self, msg: PoseWithCovarianceStamped):
         ball_point = PointStamped(header=msg.header, point=msg.pose.pose.position)
         try:
             self.ball = self.transform_to_map_frame(ball_point)
-            self.ball_covariance = msg.pose.covariance
+            self.ball_covariance = np.asarray(msg.pose.covariance, dtype=np.float64)
         except TransformException as err:
             self.logger.error(f"Could not transform ball to map frame: {err}")
 
@@ -238,16 +247,6 @@ def ball_velocity_cb(self, msg: TwistWithCovarianceStamped):
     def transform_to_map_frame(self, field, timeout_in_s=0.3):
         return self.tf_buffer.transform(field, self.map_frame, timeout=Duration(seconds=timeout_in_s))
 
-    def receive_forever(self):
-        while rclpy.ok():
-            try:
-                message = self.socket_communication.receive_message()
-            except (struct.error, socket.timeout):
-                continue
-
-            if message:
-                self.handle_message(message)
-
     def handle_message(self, string_message: bytes):
         message = Proto.Message()
         message.ParseFromString(string_message)
@@ -276,7 +275,7 @@ def is_still_valid(time: Optional[TimeMsg]) -> bool:
         message = self.protocol_converter.convert_to_message(self, msg, is_still_valid)
         proto_msg = message.SerializeToString()
         self.logger.debug(f"Sending msg with size {len(proto_msg)} bytes")
-        self.socket_communication.send_message(proto_msg)
+        self.communication.send_message(proto_msg)
 
     def create_empty_message(self, now: Time) -> Proto.Message:
         message = Proto.Message()

src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/communication.py

@@ -1,11 +1,39 @@
 import socket
+import struct
+import threading
+from abc import ABC, abstractmethod
+from typing import Callable, Optional
 
+import rclpy
+from rclpy.callback_groups import MutuallyExclusiveCallbackGroup
 from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy
+from std_msgs.msg import UInt8MultiArray
 
 from bitbots_team_communication.network import resolve_target_ip
 
 
-class SocketCommunication:
+class CommunicationBackend(ABC):
+    """Transport used to exchange serialized team communication messages with other robots."""
+
+    @abstractmethod
+    def establish_connection(self) -> None: ...
+
+    @abstractmethod
+    def is_setup(self) -> bool: ...
+
+    @abstractmethod
+    def send_message(self, message: bytes) -> None: ...
+
+    @abstractmethod
+    def start_receiving(self, callback: Callable[[bytes], None]) -> None:
+        """Start delivering incoming messages to `callback`, called with the raw bytes of each message."""
+
+    @abstractmethod
+    def close_connection(self) -> None: ...
+
+
+class SocketCommunication(CommunicationBackend):
     def __init__(self, node: Node, logger, team_id, robot_id):
         self.logger = logger
 
@@ -29,6 +57,9 @@ def __init__(self, node: Node, logger, team_id, robot_id):
             self.target_ports = [target_port]
             self.receive_port = receive_port
 
+        self._running = False
+        self._receive_thread: Optional[threading.Thread] = None
+
     def __del__(self):
         self.close_connection()
 
@@ -47,11 +78,27 @@ def get_connection(self) -> socket.socket:
         return sock
 
     def close_connection(self):
+        self._running = False
         if self.is_setup():
             self.socket.close()  # type: ignore[union-attr]
             self.logger.info("Connection closed.")
 
-    def receive_message(self) -> bytes | None:
+    def start_receiving(self, callback: Callable[[bytes], None]) -> None:
+        self._running = True
+
+        def loop():
+            while self._running and rclpy.ok():
+                try:
+                    message = self.receive_message()
+                except (struct.error, socket.timeout):
+                    continue
+                if message:
+                    callback(message)
+
+        self._receive_thread = threading.Thread(target=loop, daemon=True)
+        self._receive_thread.start()
+
+    def receive_message(self) -> Optional[bytes]:
         self.assert_is_setup()
         msg, _, flags, _ = self.socket.recvmsg(self.buffer_size)  # type: ignore[union-attr]
         is_message_truncated = flags & socket.MSG_TRUNC
@@ -76,3 +123,56 @@ def send_message(self, message):
 
     def assert_is_setup(self):
         assert self.is_setup(), "Socket is not yet initialized"
+
+
+class RosCommunication(CommunicationBackend):
+    """Exchanges team communication messages as serialized binary blobs over ROS topics.
+
+    Used instead of `SocketCommunication` in simulation, where each robot runs in its own ROS
+    domain ID (so a fixed UDP port can't be shared) and a `domain_bridge` instance per robot
+    mirrors `ros_topic_out`/`ros_topic_in` between that robot's domain and a shared hub domain,
+    emulating the UDP broadcast used between real robots.
+
+    The two topics are deliberately distinct rather than a single `bidirectional: true` bridge
+    of one topic: with more than two domains involved, a single shared topic name feeds back
+    into itself across bridges. E.g. bridge A forwards robot A's message into the hub, bridge B
+    relays it out to robot B - but robot B's bridge is *also* subscribed to that same hub-side
+    topic for its own outgoing forwarding, so it immediately re-publishes the message back into
+    the hub, where bridge A picks it up again, and so on: an unbounded loop (confirmed
+    experimentally). Separate names for the hub-bound and robot-bound directions make that
+    cycle structurally impossible.
+    """
+
+    def __init__(self, node: Node, logger):
+        self.logger = logger
+        self.node = node
+
+        out_topic: str = node.get_parameter("ros_topic_out").value
+        self.in_topic: str = node.get_parameter("ros_topic_in").value
+
+        self.qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)
+        self.publisher = node.create_publisher(UInt8MultiArray, out_topic, self.qos)
+
+    def establish_connection(self) -> None:
+        pass
+
+    def is_setup(self) -> bool:
+        return True
+
+    def send_message(self, message: bytes) -> None:
+        self.publisher.publish(UInt8MultiArray(data=list(message)))
+
+    def start_receiving(self, callback: Callable[[bytes], None]) -> None:
+        def receive_ros_message(message: UInt8MultiArray) -> None:
+            callback(bytes(message.data))
+
+        self.node.create_subscription(
+            UInt8MultiArray,
+            self.in_topic,
+            receive_ros_message,
+            qos_profile=self.qos,
+            callback_group=MutuallyExclusiveCallbackGroup(),
+        )
+
+    def close_connection(self) -> None:
+        pass

src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/state_to_message_converter.py

@@ -1,5 +1,6 @@
 import math
-from typing import Callable, Optional
+from collections.abc import Callable
+from typing import TYPE_CHECKING, Any, Optional
 
 import numpy as np
 import transforms3d
@@ -12,6 +13,13 @@
 import bitbots_team_communication.robocup_extension_pb2 as Proto  # noqa: N812
 from bitbots_msgs.msg import Strategy
 
+if TYPE_CHECKING:
+    from bitbots_team_communication.bitbots_team_communication import TeamCommunication
+else:
+    TeamCommunication = Any
+
+Covariance = Float64[np.ndarray, "36"]
+
 
 class StateToMessageConverter:
     def __init__(self, team_mapping, role_mapping, action_mapping, side_mapping):
@@ -21,7 +29,10 @@ def __init__(self, team_mapping, role_mapping, action_mapping, side_mapping):
         self.side_mapping = side_mapping
 
     def convert(
-        self, state, message: Proto.Message, is_still_valid_checker: Callable[[Optional[Time]], bool]
+        self,
+        state: TeamCommunication,
+        message: Proto.Message,
+        is_still_valid_checker: Callable[[Optional[Time]], bool],
     ) -> Proto.Message:
         def convert_gamestate(gamestate: Optional[GameState], message: Proto.Message):
             if gamestate is not None and is_still_valid_checker(gamestate.header.stamp):
@@ -68,7 +79,7 @@ def convert_target_position(target_position: Optional[PoseStamped], message):
         def convert_ball_position(
             ball_position: Optional[PointStamped],
             ball_velocity: tuple[float, float, float],
-            ball_covariance: Float64[np.ndarray, "36"],
+            ball_covariance: Covariance,
             message,
         ):
             if ball_position is not None and is_still_valid_checker(ball_position.header.stamp):
@@ -158,7 +169,7 @@ def convert_time_to_ball(
             message = convert_current_pose(state.pose, message)
         if state.cmd_vel is not None:
             message = convert_walk_command(state.cmd_vel, state.cmd_vel_time, message)
-        if convert_target_position is not None:
+        if state.move_base_goal is not None:
             message = convert_target_position(state.move_base_goal, message)
         if state.ball is not None:
             message = convert_ball_position(state.ball, state.ball_velocity, state.ball_covariance, message)
@@ -187,17 +198,15 @@ def extract_orientation_yaw_angle(self, quaternion: Quaternion):
     def convert_to_euler(self, quaternion: Quaternion):
         return transforms3d.euler.quat2euler([quaternion.w, quaternion.x, quaternion.y, quaternion.z])
 
-    def convert_to_covariance_matrix(
-        self, covariance_matrix: Proto.fmat3, row_major_covariance: Float64[np.ndarray, "36"]
-    ):
+    def convert_to_covariance_matrix(self, covariance_matrix: Proto.fmat3, row_major_covariance: Covariance):
         # ROS covariance is row-major 36 x float, while protobuf covariance
         # is column-major 9 x float [x, y, θ]
-        covariance_matrix.x.x = row_major_covariance[0]
-        covariance_matrix.y.x = row_major_covariance[1]
-        covariance_matrix.z.x = row_major_covariance[5]
-        covariance_matrix.x.y = row_major_covariance[6]
-        covariance_matrix.y.y = row_major_covariance[7]
-        covariance_matrix.z.y = row_major_covariance[11]
-        covariance_matrix.x.z = row_major_covariance[30]
-        covariance_matrix.y.z = row_major_covariance[31]
-        covariance_matrix.z.z = row_major_covariance[35]
+        covariance_matrix.x.x = float(row_major_covariance[0])
+        covariance_matrix.y.x = float(row_major_covariance[1])
+        covariance_matrix.z.x = float(row_major_covariance[5])
+        covariance_matrix.x.y = float(row_major_covariance[6])
+        covariance_matrix.y.y = float(row_major_covariance[7])
+        covariance_matrix.z.y = float(row_major_covariance[11])
+        covariance_matrix.x.z = float(row_major_covariance[30])
+        covariance_matrix.y.z = float(row_major_covariance[31])
+        covariance_matrix.z.z = float(row_major_covariance[35])