Adds HAL_Initialize to ErrorBase constructor, and makes HAL_Initialize properly reentrant (#668)

Partial fix to #663, and most likely the best we are going to be able to
get.

hal/src/main/native/athena/HAL.cpp

@@ -331,6 +331,15 @@ static bool killExistingProgram(int timeout, int mode) {
  * Call this to start up HAL. This is required for robot programs.
  */
 HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
+  static std::atomic_bool initialized{false};
+  static std::mutex initializeMutex;
+  // Initial check, as if it's true initialization has finished
+  if (initialized) return true;
+
+  std::lock_guard<std::mutex> lock(initializeMutex);
+  // Second check in case another thread was waiting
+  if (initialized) return true;
+
   setlinebuf(stdin);
   setlinebuf(stdout);
 
@@ -370,6 +379,7 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
 
   HAL_InitializeDriverStation();
 
+  initialized = true;
   return true;
 }
 

hal/src/main/native/sim/HAL.cpp

@@ -194,8 +194,19 @@ HAL_Bool HAL_GetBrownedOut(int32_t* status) {
 }
 
 HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
+  static std::atomic_bool initialized{false};
+  static std::mutex initializeMutex;
+  // Initial check, as if it's true initialization has finished
+  if (initialized) return true;
+
+  std::lock_guard<std::mutex> lock(initializeMutex);
+  // Second check in case another thread was waiting
+  if (initialized) return true;
+
   hal::RestartTiming();
   HAL_InitializeDriverStation();
+
+  initialized = true;
   return true;  // Add initialization if we need to at a later point
 }
 

wpilibc/src/main/native/cpp/ErrorBase.cpp

@@ -13,6 +13,8 @@
 #include <iomanip>
 #include <sstream>
 
+#include <HAL/HAL.h>
+
 #define WPI_ERRORS_DEFINE_STRINGS
 #include "WPIErrors.h"
 #include "llvm/SmallString.h"
@@ -23,6 +25,8 @@ using namespace frc;
 std::mutex ErrorBase::_globalErrorMutex;
 Error ErrorBase::_globalError;
 
+ErrorBase::ErrorBase() { HAL_Initialize(500, 0); }
+
 /**
  * @brief Retrieve the current error.
  * Get the current error information associated with this sensor.

wpilibc/src/main/native/include/ErrorBase.h

@@ -74,7 +74,7 @@ namespace frc {
 class ErrorBase {
   // TODO: Consider initializing instance variables and cleanup in destructor
  public:
-  ErrorBase() = default;
+  ErrorBase();
   virtual ~ErrorBase() = default;
 
   ErrorBase(const ErrorBase&) = delete;