diff --git a/hal/src/main/native/systemcore/FRCDriverStation.cpp b/hal/src/main/native/systemcore/FRCDriverStation.cpp index f5f67d60cf..396616cce2 100644 --- a/hal/src/main/native/systemcore/FRCDriverStation.cpp +++ b/hal/src/main/native/systemcore/FRCDriverStation.cpp @@ -73,6 +73,7 @@ struct SystemServerDriverStation { wpi::nt::BooleanPublisher hasUserCodeReadyPublisher; wpi::nt::BooleanSubscriber hasSetWallClockSubscriber; + wpi::nt::BooleanSubscriber serverReadySubscriber; wpi::nt::ProtobufSubscriber controlDataSubscriber; wpi::nt::ProtobufSubscriber matchInfoSubscriber; @@ -166,6 +167,22 @@ struct SystemServerDriverStation { ntInst.GetIntegerTopic(ROBOT_CURRENT_OPMODE_TRACE_PATH) .Publish(options); traceOpModePublisher.GetTopic().SetCached(false); + + serverReadySubscriber = + ntInst.GetBooleanTopic(ROBOT_SERVER_READY_PATH).Subscribe(false); + + int checkCount = 0; + while (!serverReadySubscriber.Get()) { + if (++checkCount > 500) { + fmt::print(stderr, + "Error: Waiting for server ready failed. Restarting app and " + "retrying...\n", + ROBOT_SERVER_READY_PATH); + + std::terminate(); + } + std::this_thread::sleep_for(std::chrono::milliseconds(20)); + } } void HandleListener(const wpi::nt::Event& event); diff --git a/hal/src/mrc/include/mrc/NtNetComm.h b/hal/src/mrc/include/mrc/NtNetComm.h index 003242a943..feb415a199 100644 --- a/hal/src/mrc/include/mrc/NtNetComm.h +++ b/hal/src/mrc/include/mrc/NtNetComm.h @@ -17,6 +17,7 @@ (ROBOT_CONTROL_DATA_PREFIX "JoystickDescriptors") #define ROBOT_HAS_SET_WALL_CLOCK_PATH \ (ROBOT_CONTROL_DATA_PREFIX "HasSetWallClock") +#define ROBOT_SERVER_READY_PATH (ROBOT_CONTROL_DATA_PREFIX "ServerReady") #define ROBOT_STATUS_DATA_PREFIX "/Netcomm/Status/" #define ROBOT_HAS_USER_CODE_PATH (ROBOT_STATUS_DATA_PREFIX "HasUserCode")