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:
@@ -81,6 +81,15 @@ def generate_domain_bridge_config(robot_domain: int, output_dir: Path) -> Path:
         "remap": f"{namespace}/joint_command",
     }
 
+    # Team communication: bridged bidirectionally on a single shared topic. Every robot's
+    # bridge mirrors this topic between its own domain and the shared main domain, merging
+    # all robots' traffic there and relaying it back out to everyone, including the sender
+    # (domain_bridge's bidirectional mode guards against the bridge looping on itself).
+    config["topics"]["team_comm_binary_transport"] = {
+        "type": "std_msgs/msg/UInt8MultiArray",
+        "bidirectional": True,
+    }
+
     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)

src/bitbots_motion/bitbots_head_mover/src/move_head.cpp

@@ -450,16 +450,16 @@ class HeadMover {
   /**
    * @brief Generates a parameterized search pattern
    */
-  std::vector<std::pair<double, double>> generatePattern(
-      int line_count, double max_horizontal_angle_left, double max_horizontal_angle_right, double max_vertical_angle_up,
-      double max_vertical_angle_down,
-      double reduce_last_scanline = 1.0,
-      int interpolation_steps = 0) {
+  std::vector<std::pair<double, double>> generatePattern(int line_count, double max_horizontal_angle_left,
+                                                         double max_horizontal_angle_right,
+                                                         double max_vertical_angle_up, double max_vertical_angle_down,
+                                                         double reduce_last_scanline = 1.0,
+                                                         int interpolation_steps = 0) {
     // Store the keyframes of the search pattern
     std::vector<std::pair<double, double>> keyframes;
     // Store the state of the generation process
-    bool down_direction = true;         // true = decreasing line (toward top), false = increasing line (toward bottom)
-    bool right_side = false;            // true = right, false = left
+    bool down_direction = true;   // true = decreasing line (toward top), false = increasing line (toward bottom)
+    bool right_side = false;      // true = right, false = left
     bool right_direction = true;  // true = moving right, false = moving left; alternates per scan line
     int line = line_count - 1;
     // Calculate the number of iterations that are needed to generate the search pattern

src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py

@@ -1,7 +1,5 @@
 #!/usr/bin/env python3
 
-import socket
-import struct
 import threading
 from typing import Optional
 
@@ -26,7 +24,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 +45,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 +70,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()
@@ -99,8 +103,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):
@@ -238,16 +246,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 +274,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,47 @@ 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 a ROS topic.
+
+    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
+    bridges `ros_topic` bidirectionally between that robot's domain and a shared hub domain,
+    emulating the UDP broadcast used between real robots. As with UDP broadcast, a robot also
+    receives its own messages back; these are filtered out downstream by player/team id.
+    """
+
+    def __init__(self, node: Node, logger):
+        self.logger = logger
+        self.node = node
+
+        topic: str = node.get_parameter("ros_topic").value
+        self.qos = QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)
+        self.publisher = node.create_publisher(UInt8MultiArray, topic, self.qos)
+        self.topic = topic
+
+    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 handle_ros_message(msg: UInt8MultiArray) -> None:
+            callback(bytes(msg.data))
+
+        self.node.create_subscription(
+            UInt8MultiArray,
+            self.topic,
+            handle_ros_message,
+            qos_profile=self.qos,
+            callback_group=MutuallyExclusiveCallbackGroup(),
+        )
+
+    def close_connection(self) -> None:
+        pass

src/bitbots_team_communication/bitbots_team_communication/config/team_communication_config.yaml

@@ -1,5 +1,11 @@
 team_comm:
   ros__parameters:
+    # Which transport to use to exchange messages with other robots.
+    # "udp": broadcast/unicast over a UDP socket (default, used on real robots).
+    # "ros_topic": publish/subscribe serialized messages as ROS topics. Used in simulation,
+    # where each robot may run in its own ROS domain ID on the same host, making UDP communication with a fixed port impossible.
+    transport: udp
+
     # "auto" uses the IPv4 broadcast address of the connected Wi-Fi interface.
     # Alternatively, set a specific UDP broadcast address, e.g. 172.20.255.255.
     # Sets local mode if set to loopback (127.0.0.1)
@@ -17,6 +23,11 @@ team_comm:
       - 4003
       - 4004
 
+    # Only used when transport is "ros_topic". Messages are published and received on this
+    # single topic, bridged bidirectionally to other robots' domains (see mujoco_simulation
+    # domain bridge config). Like UDP broadcast, a robot also receives its own messages back.
+    ros_topic: team_comm_binary_transport
+
     # Rate of published messages in Hz
     rate: 2
 

src/bitbots_team_communication/bitbots_team_communication/launch/team_comm.launch

@@ -3,8 +3,19 @@
     <!-- Get launch params-->
     <arg name="sim" default="false" description="true: activates simulation time" />
 
-    <node pkg="bitbots_team_communication" exec="team_comm.py" output="screen">
-        <param from="$(find-pkg-share bitbots_team_communication)/config/team_communication_config.yaml"/>
-        <param name="use_sim_time" value="$(var sim)" />
-    </node>
+    <!-- In simulation, robots may run in separate ROS domain IDs on the same host, which a fixed UDP port can't
+         span, so team communication is exchanged over a domain-bridged ROS topic instead. -->
+    <group if="$(var sim)">
+        <node pkg="bitbots_team_communication" exec="team_comm.py" output="screen">
+            <param from="$(find-pkg-share bitbots_team_communication)/config/team_communication_config.yaml"/>
+            <param name="use_sim_time" value="true" />
+            <param name="transport" value="ros_topic" />
+        </node>
+    </group>
+    <group unless="$(var sim)">
+        <node pkg="bitbots_team_communication" exec="team_comm.py" output="screen">
+            <param from="$(find-pkg-share bitbots_team_communication)/config/team_communication_config.yaml"/>
+            <param name="use_sim_time" value="false" />
+        </node>
+    </group>
 </launch>