[wpiutil] Add DsClient (#3654)

This is a libuv-based implementation of the Driver Station client
for getting robot IPs from the Driver Station port 1742 server.

wpiutil/src/main/native/cpp/DsClient.cpp

@@ -0,0 +1,107 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/DsClient.h"
+
+#include <fmt/format.h>
+#include <wpi/StringExtras.h>
+#include <wpi/json.h>
+#include <wpi/uv/Tcp.h>
+#include <wpi/uv/Timer.h>
+
+#include "wpi/Logger.h"
+
+using namespace wpi;
+
+static constexpr uv::Timer::Time kReconnectTime{500};
+
+DsClient::DsClient(wpi::uv::Loop& loop, wpi::Logger& logger,
+                   const private_init&)
+    : m_logger{logger},
+      m_tcp{uv::Tcp::Create(loop)},
+      m_timer{uv::Timer::Create(loop)} {
+  m_tcp->end.connect([this] {
+    WPI_DEBUG4(m_logger, "{}", "DS connection closed");
+    clearIp();
+    // try to connect again
+    m_tcp->Reuse([this] { m_timer->Start(kReconnectTime); });
+  });
+  m_tcp->data.connect([this](wpi::uv::Buffer buf, size_t len) {
+    HandleIncoming({buf.base, len});
+  });
+  m_timer->timeout.connect([this] { Connect(); });
+  Connect();
+}
+
+DsClient::~DsClient() = default;
+
+void DsClient::Close() {
+  m_tcp->Close();
+  m_timer->Close();
+  clearIp();
+}
+
+void DsClient::Connect() {
+  auto connreq = std::make_shared<uv::TcpConnectReq>();
+  connreq->connected.connect([this] {
+    m_json.clear();
+    m_tcp->StopRead();
+    m_tcp->StartRead();
+  });
+
+  connreq->error = [this](uv::Error err) {
+    WPI_DEBUG4(m_logger, "DS connect failure: {}", err.str());
+    // try to connect again
+    m_tcp->Reuse([this] { m_timer->Start(kReconnectTime); });
+  };
+
+  WPI_DEBUG4(m_logger, "{}", "Starting DS connection attempt");
+  m_tcp->Connect("127.0.0.1", 1742, connreq);
+}
+
+void DsClient::HandleIncoming(std::string_view in) {
+  // this is very bare-bones, as there are never nested {} in these messages
+  while (!in.empty()) {
+    // if json is empty, look for the first { (and discard)
+    if (m_json.empty()) {
+      auto start = in.find('{');
+      in = wpi::slice(in, start, std::string_view::npos);
+    }
+
+    // look for the terminating } (and save)
+    auto end = in.find('}');
+    if (end == std::string_view::npos) {
+      m_json.append(in);
+      return;  // nothing left to read
+    }
+
+    // have complete json message
+    ++end;
+    m_json.append(wpi::slice(in, 0, end));
+    in = wpi::slice(in, end, std::string_view::npos);
+    ParseJson();
+    m_json.clear();
+  }
+}
+
+void DsClient::ParseJson() {
+  WPI_DEBUG4(m_logger, "DsClient JSON: {}", m_json);
+  unsigned int ip = 0;
+  try {
+    ip = wpi::json::parse(m_json).at("robotIP").get<unsigned int>();
+  } catch (wpi::json::exception& e) {
+    WPI_INFO(m_logger, "DsClient JSON error: {}", e.what());
+    return;
+  }
+
+  if (ip == 0) {
+    clearIp();
+  } else {
+    // Convert number into dotted quad
+    auto newip = fmt::format("{}.{}.{}.{}", (ip >> 24) & 0xff,
+                             (ip >> 16) & 0xff, (ip >> 8) & 0xff, ip & 0xff);
+    WPI_INFO(m_logger, "DS received server IP: {}", newip);
+    setIp(newip);
+  }
+}