diff --git a/src/bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py b/src/bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py
index a6a6de6641..b482939aad 100644
--- a/src/bitbots_misc/bitbots_bringup/launch/mujoco_simulation.launch.py
+++ b/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
diff --git a/src/bitbots_misc/bitbots_bringup/package.xml b/src/bitbots_misc/bitbots_bringup/package.xml
index 2ef2d73161..b6b15302d5 100644
--- a/src/bitbots_misc/bitbots_bringup/package.xml
+++ b/src/bitbots_misc/bitbots_bringup/package.xml
@@ -28,6 +28,7 @@
bitbots_robot_description
bitbots_utils
bitbots_vision
+ domain_bridge
foxglove_bridge
game_controller_hsl
humanoid_base_footprint
diff --git a/src/bitbots_simulation/bitbots_mujoco_sim/xml/pi_plus.xml b/src/bitbots_simulation/bitbots_mujoco_sim/xml/pi_plus.xml
index 7989100c3b..19146f6dca 100644
--- a/src/bitbots_simulation/bitbots_mujoco_sim/xml/pi_plus.xml
+++ b/src/bitbots_simulation/bitbots_mujoco_sim/xml/pi_plus.xml
@@ -78,32 +78,32 @@
-
+
-
+
-
+
-
+
-
+
-
+
-
+
@@ -114,23 +114,23 @@
-
+
-
+
-
+
-
+
-
+
@@ -141,16 +141,16 @@
-
+
-
+
-
+
@@ -159,32 +159,32 @@
-
+
-
+
-
+
-
+
-
+
-
+
@@ -199,32 +199,32 @@
-
+
-
+
-
+
-
+
-
+
-
+
diff --git a/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py b/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py
index d52c6e7a11..6c41c22669 100755
--- a/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py
+++ b/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()
diff --git a/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/communication.py b/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/communication.py
index 54f749caf7..b2f8e9d008 100644
--- a/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/communication.py
+++ b/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
diff --git a/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/state_to_message_converter.py b/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/state_to_message_converter.py
index 1d821e48a0..f18b8618cd 100644
--- a/src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/converter/state_to_message_converter.py
+++ b/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])
diff --git a/src/bitbots_team_communication/bitbots_team_communication/config/team_communication_config.yaml b/src/bitbots_team_communication/bitbots_team_communication/config/team_communication_config.yaml
index cbc3166f1a..ffbf03a259 100644
--- a/src/bitbots_team_communication/bitbots_team_communication/config/team_communication_config.yaml
+++ b/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,13 @@ team_comm:
- 4003
- 4004
+ # Only used when transport is "ros_topic". Outgoing messages are published on
+ # ros_topic_out and incoming messages are read from ros_topic_in. These must be
+ # different topics - see RosCommunication in communication.py for why a single
+ # shared topic causes an unbounded forwarding loop across robots' domain bridges.
+ ros_topic_out: team_comm_binary_transport/out
+ ros_topic_in: team_comm_binary_transport/in
+
# Rate of published messages in Hz
rate: 2
diff --git a/src/bitbots_team_communication/bitbots_team_communication/launch/team_comm.launch b/src/bitbots_team_communication/bitbots_team_communication/launch/team_comm.launch
index 2bb22f9c49..c10960b62d 100644
--- a/src/bitbots_team_communication/bitbots_team_communication/launch/team_comm.launch
+++ b/src/bitbots_team_communication/bitbots_team_communication/launch/team_comm.launch
@@ -3,8 +3,19 @@
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+