[sim] Add WebSocket extension (client/server) (#2589)

This allows access to HAL-level simulation data via a WebSocket connection.

The server additionally serves local files.

The following environment variables can be used for configuration:
HALSIMWS_USERROOT (server) - local directory to use for file serving for /user/ URIs, defaults to ./sim/user
HALSIMWS_SYSROOT (server) - local directory to use for file serving for all other URIs, defaults to ./sim
HALSIMWS_URI (client or server) - WebSocket URI, defaults to /wpilibws
HALSIMWS_PORT (client or server) - port number, defaults to 8080
HALSIMWS_HOST (client) - host to connect to, defaults to localhost

Co-authored-by: Zhiquan Yeo <zyeo8@bloomberg.net>
Co-authored-by: Peter Johnson <johnson.peter@gmail.com>
Co-authored-by: jpokornyiii <jpokornyiii@gmail.com>

simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClient.cpp

@@ -0,0 +1,194 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "HALSimWSClient.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/raw_ostream.h>
+#include <wpi/uv/util.h>
+
+#include "HALSimWSClientConnection.h"
+
+static constexpr int kTcpConnectAttemptTimeout = 1000;
+
+namespace uv = wpi::uv;
+
+namespace wpilibws {
+
+std::shared_ptr<HALSimWS> HALSimWS::g_instance;
+
+bool HALSimWS::Initialize() {
+  const char* host = std::getenv("HALSIMWS_HOST");
+  if (host != NULL) {
+    m_host = host;
+  } else {
+    m_host = "localhost";
+  }
+
+  const char* port = std::getenv("HALSIMWS_PORT");
+  if (port != NULL) {
+    try {
+      m_port = std::stoi(port);
+    } catch (const std::invalid_argument& err) {
+      wpi::errs() << "Error decoding HALSIMWS_PORT (" << err.what() << ")\n";
+      return false;
+    }
+  } else {
+    m_port = 8080;
+  }
+
+  const char* uri = std::getenv("HALSIMWS_URI");
+  if (uri != NULL) {
+    m_uri = uri;
+  } else {
+    m_uri = "/wpilibws";
+  }
+
+  m_loop = uv::Loop::Create();
+  if (!m_loop) {
+    return false;
+  }
+
+  m_loop->error.connect([](uv::Error err) {
+    wpi::errs() << "HALSim WS Client libuv Error: " << err.str() << "\n";
+  });
+
+  m_tcp_client = uv::Tcp::Create(m_loop);
+  if (!m_tcp_client) {
+    return false;
+  }
+
+  m_tcp_client->SetNoDelay(true);
+
+  // Hook up TCP client events
+  m_tcp_client->error.connect(
+      [this, socket = m_tcp_client.get()](wpi::uv::Error err) {
+        if (m_tcp_connected) {
+          m_tcp_connected = false;
+          m_connect_attempts = 0;
+          m_loop->Stop();
+          return;
+        }
+
+        // If we weren't previously connected, attempt a reconnection
+        m_connect_timer->Start(uv::Timer::Time(kTcpConnectAttemptTimeout));
+      });
+
+  m_tcp_client->closed.connect(
+      []() { wpi::outs() << "TCP connection closed\n"; });
+
+  // Set up the connection timer
+  m_connect_timer = uv::Timer::Create(m_loop);
+  if (!m_connect_timer) {
+    return false;
+  }
+
+  wpi::outs() << "HALSimWS Initialized\n";
+  wpi::outs() << "Will attempt to connect to: " << m_host << ":" << m_port
+              << " " << m_uri << "\n";
+
+  return true;
+}
+
+void HALSimWS::Main(void* param) {
+  GetInstance()->MainLoop();
+  SetInstance(nullptr);
+}
+
+void HALSimWS::MainLoop() {
+  // Set up the timer to attempt connection
+  m_connect_timer->timeout.connect([this] { AttemptConnect(); });
+
+  // Run the initial connect immediately
+  m_connect_timer->Start(uv::Timer::Time(0));
+
+  m_loop->Run();
+}
+
+void HALSimWS::AttemptConnect() {
+  m_connect_attempts++;
+
+  wpi::outs() << "Connection Attempt " << m_connect_attempts << "\n";
+
+  struct sockaddr_in dest;
+  uv::NameToAddr(m_host, m_port, &dest);
+
+  m_tcp_client->Connect(dest, [this, socket = m_tcp_client.get()]() {
+    m_tcp_connected = true;
+    auto wsConn = std::make_shared<HALSimWSClientConnection>(m_tcp_client);
+
+    wsConn->Initialize();
+  });
+}
+
+void HALSimWS::Exit(void* param) {
+  auto inst = GetInstance();
+  if (!inst) {
+    return;
+  }
+
+  auto loop = inst->m_loop;
+  loop->Walk([](uv::Handle& h) {
+    h.SetLoopClosing(true);
+    h.Close();
+  });
+}
+
+bool HALSimWS::RegisterWebsocket(
+    std::shared_ptr<HALSimBaseWebSocketConnection> hws) {
+  if (m_hws.lock()) {
+    return false;
+  }
+
+  m_hws = hws;
+
+  m_simDevicesProvider.OnNetworkConnected(hws);
+
+  m_providers.ForEach([hws](std::shared_ptr<HALSimWSBaseProvider> provider) {
+    provider->OnNetworkConnected(hws);
+  });
+
+  return true;
+}
+
+void HALSimWS::CloseWebsocket(
+    std::shared_ptr<HALSimBaseWebSocketConnection> hws) {
+  // Inform the providers that they need to cancel callbacks
+  m_simDevicesProvider.OnNetworkDisconnected();
+
+  m_providers.ForEach([](std::shared_ptr<HALSimWSBaseProvider> provider) {
+    provider->OnNetworkDisconnected();
+  });
+
+  if (hws == m_hws.lock()) {
+    m_hws.reset();
+  }
+}
+
+void HALSimWS::OnNetValueChanged(const wpi::json& msg) {
+  // Look for "type" and "device" fields so that we can
+  // generate the key
+
+  try {
+    auto& type = msg.at("type").get_ref<const std::string&>();
+    auto& device = msg.at("device").get_ref<const std::string&>();
+
+    wpi::SmallString<64> key;
+    key.append(type);
+    key.append("/");
+    key.append(device);
+
+    auto provider = m_providers.Get(key.str());
+    if (provider) {
+      provider->OnNetValueChanged(msg.at("data"));
+    }
+  } catch (wpi::json::exception& e) {
+    wpi::errs() << "Error with incoming message: " << e.what() << "\n";
+  }
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClientConnection.cpp

@@ -0,0 +1,122 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "HALSimWSClientConnection.h"
+
+#include <wpi/raw_ostream.h>
+#include <wpi/raw_uv_ostream.h>
+
+#include "HALSimWSClient.h"
+
+namespace uv = wpi::uv;
+
+namespace wpilibws {
+
+void HALSimWSClientConnection::Initialize() {
+  // Get a shared pointer to ourselves
+  auto self = this->shared_from_this();
+
+  auto hws = HALSimWS::GetInstance();
+  std::string reqHost =
+      hws->GetTargetHost() + ":" + std::to_string(hws->GetTargetPort());
+
+  auto ws =
+      wpi::WebSocket::CreateClient(*m_stream, hws->GetTargetUri(), reqHost);
+
+  ws->SetData(self);
+
+  m_websocket = ws.get();
+
+  // Hook up events
+  m_websocket->open.connect_extended([this](auto conn, wpi::StringRef) {
+    conn.disconnect();
+
+    m_buffers = std::make_unique<BufferPool>();
+
+    m_exec =
+        UvExecFunc::Create(m_stream->GetLoop(), [](auto out, LoopFunc func) {
+          func();
+          out.set_value();
+        });
+
+    auto hws = HALSimWS::GetInstance();
+    if (!hws) {
+      wpi::errs() << "Unable to get hws instance\n";
+      return;
+    }
+
+    if (!hws->RegisterWebsocket(shared_from_this())) {
+      wpi::errs() << "Unable to register websocket\n";
+      return;
+    }
+
+    m_ws_connected = true;
+    wpi::outs() << "HALSimWS: WebSocket Connected\n";
+  });
+
+  m_websocket->text.connect([this](wpi::StringRef msg, bool) {
+    auto hws = HALSimWS::GetInstance();
+    if (!m_ws_connected || !hws) {
+      return;
+    }
+
+    wpi::json j;
+    try {
+      j = wpi::json::parse(msg);
+    } catch (const wpi::json::parse_error& e) {
+      std::string err("JSON parse failed: ");
+      err += e.what();
+      wpi::errs() << err << "\n";
+      m_websocket->Fail(1003, err);
+      return;
+    }
+
+    hws->OnNetValueChanged(j);
+  });
+
+  m_websocket->closed.connect([this](uint16_t, wpi::StringRef) {
+    if (m_ws_connected) {
+      wpi::outs() << "HALSimWS: Websocket Disconnected\n";
+      m_ws_connected = false;
+
+      auto hws = HALSimWS::GetInstance();
+      if (hws) {
+        hws->CloseWebsocket(shared_from_this());
+      }
+    }
+  });
+}
+
+void HALSimWSClientConnection::OnSimValueChanged(const wpi::json& msg) {
+  if (msg.empty()) {
+    return;
+  }
+  wpi::SmallVector<uv::Buffer, 4> sendBufs;
+  wpi::raw_uv_ostream os{sendBufs, [this]() -> uv::Buffer {
+                           std::lock_guard lock(m_buffers_mutex);
+                           return m_buffers->Allocate();
+                         }};
+
+  os << msg;
+
+  // Call the websocket send function on the uv loop
+  m_exec->Call([this, sendBufs]() mutable {
+    m_websocket->SendText(sendBufs, [this](auto bufs, wpi::uv::Error err) {
+      {
+        std::lock_guard lock(m_buffers_mutex);
+        m_buffers->Release(bufs);
+      }
+
+      if (err) {
+        wpi::errs() << err.str() << "\n";
+        wpi::errs().flush();
+      }
+    });
+  });
+}
+
+}  // namespace wpilibws

simulation/halsim_ws_client/src/main/native/cpp/main.cpp

@@ -0,0 +1,67 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <WSProviderContainer.h>
+#include <WSProvider_Analog.h>
+#include <WSProvider_DIO.h>
+#include <WSProvider_DriverStation.h>
+#include <WSProvider_Encoder.h>
+#include <WSProvider_Joystick.h>
+#include <WSProvider_PWM.h>
+#include <WSProvider_Relay.h>
+#include <WSProvider_RoboRIO.h>
+#include <WSProvider_SimDevice.h>
+#include <WSProvider_dPWM.h>
+#include <hal/Main.h>
+#include <wpi/raw_ostream.h>
+
+#include "HALSimWSClient.h"
+
+using namespace wpilibws;
+
+static ProviderContainer providers;
+static HALSimWSProviderSimDevices simDevices(providers);
+
+extern "C" {
+#if defined(WIN32) || defined(_WIN32)
+__declspec(dllexport)
+#endif
+
+    int HALSIM_InitExtension(void) {
+  wpi::outs() << "HALSim WS Client Extension Initializing\n";
+
+  auto hws = std::make_shared<HALSimWS>(providers, simDevices);
+  HALSimWS::SetInstance(hws);
+
+  if (!hws->Initialize()) {
+    return -1;
+  }
+
+  WSRegisterFunc registerFunc = [&](auto key, auto provider) {
+    providers.Add(key, provider);
+  };
+
+  HALSimWSProviderAnalogIn::Initialize(registerFunc);
+  HALSimWSProviderAnalogOut::Initialize(registerFunc);
+  HALSimWSProviderDIO::Initialize(registerFunc);
+  HALSimWSProviderDigitalPWM::Initialize(registerFunc);
+  HALSimWSProviderDriverStation::Initialize(registerFunc);
+  HALSimWSProviderEncoder::Initialize(registerFunc);
+  HALSimWSProviderJoystick::Initialize(registerFunc);
+  HALSimWSProviderPWM::Initialize(registerFunc);
+  HALSimWSProviderRelay::Initialize(registerFunc);
+  HALSimWSProviderRoboRIO::Initialize(registerFunc);
+
+  simDevices.Initialize(hws->GetLoop());
+
+  HAL_SetMain(nullptr, HALSimWS::Main, HALSimWS::Exit);
+
+  wpi::outs() << "HALSim WS Client Extension Initialized\n";
+  return 0;
+}
+
+}  // extern "C"