[wpilib] Wait for expansion hub connected for up to half a second during boot (#8715)

Closes #8714

wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHub.cpp

@@ -4,7 +4,9 @@
 
 #include "wpi/hardware/expansionhub/ExpansionHub.hpp"
 
+#include <chrono>
 #include <memory>
+#include <thread>
 
 #include "wpi/hal/UsageReporting.hpp"
 #include "wpi/hardware/expansionhub/ExpansionHubMotor.hpp"
@@ -12,6 +14,7 @@
 #include "wpi/nt/BooleanTopic.hpp"
 #include "wpi/system/Errors.hpp"
 #include "wpi/system/SystemServer.hpp"
+#include "wpi/system/Timer.hpp"
 
 using namespace wpi;
 
@@ -26,6 +29,16 @@ class ExpansionHub::DataStore {
     m_hubConnectedSubscriber =
         systemServer.GetBooleanTopic(fmt::format("/rhsp/{}/connected", usbId))
             .Subscribe(false);
+
+    // Wait up to half a second for connected to come up, using a poll loop to
+    // ensure we don't block.
+    auto startTime = Timer::GetMonotonicTimestamp();
+    while (Timer::GetMonotonicTimestamp() - startTime < 0.5_s) {
+      if (m_hubConnectedSubscriber.Get(false)) {
+        break;
+      }
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
   }
 
   DataStore(DataStore&) = delete;

wpilibj/src/main/java/org/wpilib/hardware/expansionhub/ExpansionHub.java

@@ -8,6 +8,7 @@ import org.wpilib.hardware.hal.HAL;
 import org.wpilib.networktables.BooleanSubscriber;
 import org.wpilib.networktables.NetworkTableInstance;
 import org.wpilib.system.SystemServer;
+import org.wpilib.system.Timer;
 
 /** This class controls a REV ExpansionHub plugged in over USB to Systemcore. */
 public class ExpansionHub implements AutoCloseable {
@@ -28,6 +29,16 @@ public class ExpansionHub implements AutoCloseable {
 
       m_hubConnectedSubscriber =
           systemServer.getBooleanTopic("/rhsp/" + usbId + "/connected").subscribe(false);
+
+      // Wait up to half a second for connected to come up, using a poll loop to
+      // ensure we don't block.
+      double startTime = Timer.getMonotonicTimestamp();
+      while (Timer.getMonotonicTimestamp() - startTime < 0.5) {
+        if (m_hubConnectedSubscriber.get(false)) {
+          break;
+        }
+        Timer.delay(0.01);
+      }
     }
 
     @Override