Get Simulation to work with multiple robots

src/bitbots_misc/bitbots_bringup/launch/motion_standalone.launch

@@ -5,7 +5,7 @@
     <arg name="torqueless_mode" default="false" description="start without torque, for example for testing the falling detection"/>
     <arg name="tts" default="true" description="Whether to enable text-to-speech"/>
 
-    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch">
+    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch.py">
         <arg name="sim" value="$(var sim)"/>
     </include>
 

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:
@@ -92,7 +92,7 @@ def generate_world_xml(num_robots: int, package_share: str, robot_type: str) ->
     """Generate MuJoCo world XML with the correct number of robots."""
     template_path = Path(package_share) / "xml" / "kid_field.xml"
     output_path = Path(package_share) / "xml" / "generated_world.xml"
-    offset = 6 * (
+    offset = 4 * (
         1 / num_robots
     )  # this makes the offset be the default value when there are 4 robots and increse the less robots there are
 

src/bitbots_misc/bitbots_bringup/launch/teamplayer.launch

@@ -21,7 +21,7 @@
 
 
     <!-- load the global parameters -->
-    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch">
+    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch.py">
         <arg name="sim" value="$(var sim)" />
         <arg name="fieldname" value="$(var fieldname)" />
     </include>

src/bitbots_misc/bitbots_bringup/launch/vision_standalone.launch

@@ -8,7 +8,7 @@
     <arg if="$(var sim)" name="fieldname" default="hsl_kid" description="Loads field settings" />
 
     <!-- Load the global parameters-->
-    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch">
+    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch.py">
         <arg name="sim" value="$(var sim)" />
         <arg name="fieldname" value="$(var fieldname)" />
     </include>

src/bitbots_misc/bitbots_bringup/launch/visualization.launch

@@ -8,7 +8,7 @@
     <arg name="fieldname" default="labor" description="Loads field settings" />
 
     <!-- load the global parameters -->
-    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch" />
+    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch.py" />
 
     <!-- publish dummy imu -->
     <node name="dummy_imu" pkg="bitbots_utils" exec="dummy_imu.py" />

src/bitbots_misc/bitbots_parameter_blackboard/config/sim_game_settings_11.yaml

@@ -0,0 +1,5 @@
+parameter_blackboard:
+  ros__parameters:
+    bot_id: 1
+    position_number: 0
+    role: offense

src/bitbots_misc/bitbots_parameter_blackboard/config/sim_game_settings_12.yaml

@@ -0,0 +1,5 @@
+parameter_blackboard:
+  ros__parameters:
+    bot_id: 2
+    position_number: 0
+    role: goalie

src/bitbots_misc/bitbots_parameter_blackboard/config/sim_game_settings_13.yaml

@@ -0,0 +1,5 @@
+parameter_blackboard:
+  ros__parameters:
+    bot_id: 3
+    position_number: 0
+    role: defense

src/bitbots_misc/bitbots_parameter_blackboard/config/sim_game_settings_14.yaml

@@ -0,0 +1,5 @@
+parameter_blackboard:
+  ros__parameters:
+    bot_id: 4
+    position_number: 1
+    role: defense

src/bitbots_misc/bitbots_parameter_blackboard/launch/parameter_blackboard.launch.py

@@ -0,0 +1,54 @@
+from __future__ import annotations
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import AnyLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterFile
+
+
+def generate_launch_description() -> LaunchDescription:
+    package_name = "bitbots_parameter_blackboard"
+    package_share = get_package_share_directory(package_name)
+
+    sim = LaunchConfiguration("sim")
+    field_name = PythonExpression(["'hsl_kid' if '", sim, "' == 'true' else 'labor'"])
+
+    parameters = [
+        {"simulation_active": sim},
+        {"use_sim_time": sim},
+        {"field.name": field_name},
+        ParameterFile(PathJoinSubstitution([package_share, "config", "fields", field_name, "config.yaml"])),
+        ParameterFile(PathJoinSubstitution([package_share, "config", "global_parameters.yaml"])),
+        ParameterFile(PathJoinSubstitution([package_share, "config", "game_settings.yaml"])),
+    ]
+
+    robot_domain = os.environ.get("ROS_DOMAIN_ID")
+    if robot_domain is not None:
+        parameters.append(
+            ParameterFile(
+                PathJoinSubstitution([package_share, "config", f"sim_game_settings_{int(robot_domain)}.yaml"])
+            )
+        )
+
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument("sim", default_value="false"),
+            IncludeLaunchDescription(
+                AnyLaunchDescriptionSource(
+                    PathJoinSubstitution([get_package_share_directory("bitbots_utils"), "launch", "welcome.launch"])
+                )
+            ),
+            Node(
+                package="demo_nodes_cpp",
+                executable="parameter_blackboard",
+                name="parameter_blackboard",
+                arguments=["--ros-args", "--log-level", "WARN"],
+                parameters=parameters,
+            ),
+        ]
+    )

src/bitbots_motion/bitbots_animation_server/launch/test.launch

@@ -5,7 +5,7 @@
     <include file="$(find-pkg-share livelybot_bringup)/launch/lowlevel.launch" />
   </group>
 
-  <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch" />
+  <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch.py" />
   <include file="$(find-pkg-share bitbots_robot_description)/launch/load_robot_description.launch" />
 
   <node pkg="bitbots_animation_server" exec="animation_node" output="screen"/>

src/bitbots_motion/bitbots_hcm/launch/test.launch

@@ -1,7 +1,7 @@
 <launch>
     <arg name="sim" default="false" description="Disables checks for hardware, since we are in simulation."/>
 
-    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch">
+    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch.py">
         <arg name="sim" value="$(var sim)"/>
     </include>
 

src/bitbots_motion/bitbots_head_mover/launch/head_mover_standalone.launch

@@ -2,7 +2,7 @@
 <launch>
     <arg name="sim" default="false" />
 
-    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch">
+    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch.py">
         <arg name="sim" value="$(var sim)"/>
     </include>
 

src/bitbots_motion/bitbots_head_mover/launch/test.launch

@@ -9,7 +9,7 @@
         </group>
     </group>
 
-    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch" >
+    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch.py" >
         <arg name="sim" value="$(var sim)"/>
     </include>
 

src/bitbots_robot/piplus_description/launch/standalone.launch

@@ -2,7 +2,7 @@
   <arg name="js_pub" default="true" description="Whether to run the joint state publisher"/>
   <group>
     <include file="$(find-pkg-share piplus_description)/launch/rviz.launch"/>
-    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch"/>
+    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch.py"/>
     <include file="$(find-pkg-share bitbots_robot_description)/launch/load_robot_description.launch"/>
   </group>
 

src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication.py

@@ -19,6 +19,7 @@
 from rclpy.experimental.events_executor import EventsExecutor
 from rclpy.node import Node
 from rclpy.time import Time
+from rosgraph_msgs.msg import Clock
 from soccer_vision_3d_msgs.msg import Robot, RobotArray
 from std_msgs.msg import Float32, Header
 from tf2_geometry_msgs import PointStamped, PoseStamped
@@ -66,9 +67,24 @@ def __init__(self):
         self.run_spin_in_thread()
         self.try_to_establish_connection()
 
-        self.node.create_timer(1 / self.rate, self.send_message, callback_group=MutuallyExclusiveCallbackGroup())
+        # See https://github.com/ros2/rclcpp/issues/2535
+        # In sim, the builtin timers may not work correctly if there is a high load of callbacks
+        # This can lead to breaks and bursts which is unacceptable for this usecase
+        # Instead we manually implement a timer
+        if self.node.get_parameter("use_sim_time").value:
+            self.node.create_subscription(
+                Clock, "/clock", self.clock_cb, callback_group=MutuallyExclusiveCallbackGroup(), qos_profile=1
+            )
+            self.next_send_time = self.node.get_clock().now() + Duration(seconds=1 / self.rate)
+        else:
+            self.node.create_timer(1 / self.rate, self.send_message, callback_group=MutuallyExclusiveCallbackGroup())
         self.receive_forever()
 
+    def clock_cb(self, msg):
+        if Time.from_msg(msg.clock) >= self.next_send_time:
+            self.send_message()
+            self.next_send_time = Time.from_msg(msg.clock) + Duration(seconds=1 / self.rate)
+
     def spin(self):
         executor = EventsExecutor()
         executor.add_node(self.node)

src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/communication.py

@@ -12,7 +12,8 @@ def __init__(self, node: Node, logger, team_id, robot_id):
         self.buffer_size: int = 1024
         self.socket: socket.socket | None = None
         configured_target_ip: str = node.get_parameter("target_ip").value
-        self.target_ip, wifi_interface = resolve_target_ip(configured_target_ip)
+        in_sim: bool = node.get_parameter("use_sim_time").value
+        self.target_ip, wifi_interface = resolve_target_ip(configured_target_ip, in_sim)
         if wifi_interface is not None:
             self.logger.info(
                 f"Using Wi-Fi broadcast address {self.target_ip} from interface {wifi_interface} for team communication"

src/bitbots_team_communication/bitbots_team_communication/bitbots_team_communication/network.py

@@ -11,10 +11,13 @@ class WifiInterfaceError(RuntimeError):
     pass
 
 
-def resolve_target_ip(configured_target_ip: str) -> tuple[IPv4Address, str | None]:
+def resolve_target_ip(configured_target_ip: str, in_sim: bool) -> tuple[IPv4Address, str | None]:
     if configured_target_ip.strip().lower() != "auto":
         return IPv4Address(configured_target_ip), None
 
+    if in_sim:
+        return IPv4Address("127.0.0.1"), None
+
     return get_wifi_broadcast_address()
 
 

src/bitbots_team_communication/bitbots_team_communication/launch/team_comm_standalone.launch

@@ -4,7 +4,7 @@
     <arg name="sim" default="false" description="true: activates simulation time" />
 
     <!-- load the global parameters -->
-    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch" />
+    <include file="$(find-pkg-share bitbots_parameter_blackboard)/launch/parameter_blackboard.launch.py" />
 
     <node pkg="bitbots_team_communication" exec="team_comm.py" output="screen">
         <param from="$(find-pkg-share bitbots_team_communication)/config/team_communication_config.yaml"/>

src/bitbots_team_communication/bitbots_team_communication/test/network/test_network.py

@@ -10,7 +10,17 @@
 def test_resolve_manual_target_ip(mocker):
     auto_detection = mocker.patch.object(network, "get_wifi_broadcast_address")
 
-    target_ip, interface = network.resolve_target_ip("10.0.6.255")
+    target_ip, interface = network.resolve_target_ip("10.0.6.255", in_sim=False)
+
+    assert target_ip == IPv4Address("10.0.6.255")
+    assert interface is None
+    auto_detection.assert_not_called()
+
+
+def test_resolve_manual_target_ip_ignores_sim(mocker):
+    auto_detection = mocker.patch.object(network, "get_wifi_broadcast_address")
+
+    target_ip, interface = network.resolve_target_ip("10.0.6.255", in_sim=True)
 
     assert target_ip == IPv4Address("10.0.6.255")
     assert interface is None
@@ -24,12 +34,22 @@ def test_resolve_auto_target_ip(mocker):
         return_value=(IPv4Address("10.0.6.255"), "wlp2s0"),
     )
 
-    target_ip, interface = network.resolve_target_ip("auto")
+    target_ip, interface = network.resolve_target_ip("auto", in_sim=False)
 
     assert target_ip == IPv4Address("10.0.6.255")
     assert interface == "wlp2s0"
 
 
+def test_resolve_auto_target_ip_in_sim_uses_localhost(mocker):
+    auto_detection = mocker.patch.object(network, "get_wifi_broadcast_address")
+
+    target_ip, interface = network.resolve_target_ip("auto", in_sim=True)
+
+    assert target_ip == IPv4Address("127.0.0.1")
+    assert interface is None
+    auto_detection.assert_not_called()
+
+
 def test_auto_target_ip_errors_without_wifi_interface(tmp_path, monkeypatch):
     monkeypatch.setattr(network, "SYS_CLASS_NET", tmp_path)
     (tmp_path / "eth0").mkdir()

src/lib/udp_bridge/launch/send_standalone.launch

@@ -1,5 +1,5 @@
 <launch>
     <!-- load the global parameters -->
-    <include file="$(find-pkg-share bitbots_utils)/launch/parameter_blackboard.launch" />
+    <include file="$(find-pkg-share bitbots_utils)/launch/parameter_blackboard.launch.py" />
     <include file="$(find-pkg-share udp_bridge)/launch/sender.launch" />
 </launch>