fix(autoware_fault_injection): wait for endpoint discovery before publishing in tests

The test_receive_multiple_message_simultaneously test was failing on
Jazzy because messages were published before DDS endpoint discovery
completed. Replace the single spin_once with explicit discovery polling
using get_subscription_count and count_publishers.

Signed-off-by: Mete Fatih Cırıt <mfc@autoware.org>

Author: xmfcx

simulator/autoware_fault_injection/test/test_fault_injection_node.test.py

@@ -68,12 +68,26 @@ def setUp(self):
         self.test_node = rclpy.create_node("test_node")
         self.event_name = "cpu_temperature_is_high"
         self.evaluation_time = 0.5  # 500ms
+        self.discovery_timeout = 5.0  # seconds
         self.input_diagnostics_topic = "/diagnostics"
         self.output_diagnostics_topic = "/diagnostics/fault_injection"
 
     def tearDown(self):
         self.test_node.destroy_node()
 
+    def _wait_for_discovery(self, publishers, subscribers=None):
+        """Wait until all publishers have matched with at least one subscriber and vice versa."""
+        end_time = time.time() + self.discovery_timeout
+        while time.time() < end_time:
+            pubs_matched = all(pub.get_subscription_count() > 0 for pub in publishers)
+            subs_matched = subscribers is None or all(
+                self.test_node.count_publishers(topic) > 0 for topic in subscribers
+            )
+            if pubs_matched and subs_matched:
+                return
+            rclpy.spin_once(self.test_node, timeout_sec=0.1)
+        self.fail("Timed out waiting for endpoint discovery")
+
     @staticmethod
     def print_message(stat):
         logger.debug("===========================")
@@ -110,6 +124,12 @@ def test_node_link(self):
             DiagnosticArray, self.output_diagnostics_topic, lambda msg: msg_buffer.append(msg), 10
         )
 
+        # Wait for endpoint discovery before testing.
+        self._wait_for_discovery(
+            publishers=[pub_events, pub_diagnostics],
+            subscribers=[self.output_diagnostics_topic],
+        )
+
         # Test init state.
         # Expect fault_injection_node does not publish diagnostics without input.
         end_time = time.time() + self.evaluation_time
@@ -152,13 +172,19 @@ def test_receive_multiple_message_simultaneously(self):
             DiagnosticArray, self.output_diagnostics_topic, lambda msg: msg_buffer.append(msg), 10
         )
 
-        # Call spin_once() so that the publisher publish messages simultaneously
         pub_events_1 = self.test_node.create_publisher(SimulationEvents, "/simulation/events", 10)
         pub_events_2 = self.test_node.create_publisher(SimulationEvents, "/simulation/events", 10)
         pub_diagnostics = self.test_node.create_publisher(
             DiagnosticArray, self.input_diagnostics_topic, 10
         )
-        rclpy.spin_once(self.test_node, timeout_sec=0.1)
+
+        # Wait for endpoint discovery before publishing.
+        self._wait_for_discovery(
+            publishers=[pub_events_1, pub_events_2, pub_diagnostics],
+            subscribers=[self.output_diagnostics_topic],
+        )
+
+        # Publish messages simultaneously
         input_msg = DiagnosticArray()
         input_msg.status = [
             DiagnosticStatus(name=": CPU Load Average", level=DiagnosticStatus.OK, message="OK"),