Add FRC Driver Station connection support.

The 2017 FRC Driver Station supports getting the robot IP via a TCP connection that returns JSON. Use this to support overriding the server IP address used for client connections. Default to using this approach for client connections in the NetworkTable interfaces. Add support for setting the server address without stopping and restarting the client. SetTeam now also round-robins by default.
2026-07-05 03:21:42 +00:00 · 2016-11-04 16:01:42 -07:00
parent 34acd9d47c
commit 77edf1e103
15 changed files with 538 additions and 66 deletions
--- a/src/Dispatcher.cpp
+++ b/src/Dispatcher.cpp
@@ -26,16 +26,16 @@ void Dispatcher::StartServer(llvm::StringRef persist_filename,
          static_cast<int>(port), listen_address, Logger::GetInstance())));
 }

-void Dispatcher::StartClient(const char* server_name, unsigned int port) {
+void Dispatcher::SetServer(const char* server_name, unsigned int port) {
  std::string server_name_copy(server_name);
-  DispatcherBase::StartClient([=]() -> std::unique_ptr<wpi::NetworkStream> {
+  SetConnector([=]() -> std::unique_ptr<wpi::NetworkStream> {
    return wpi::TCPConnector::connect(server_name_copy.c_str(),
                                      static_cast<int>(port),
                                      Logger::GetInstance(), 1);
  });
 }

-void Dispatcher::StartClient(
+void Dispatcher::SetServer(
    ArrayRef<std::pair<StringRef, unsigned int>> servers) {
  std::vector<Connector> connectors;
  for (const auto& server : servers) {
@@ -47,9 +47,20 @@ void Dispatcher::StartClient(
                                        Logger::GetInstance(), 1);
    });
  }
-  DispatcherBase::StartClient(std::move(connectors));
+  SetConnector(std::move(connectors));
 }

+void Dispatcher::SetServerOverride(const char* server_name, unsigned int port) {
+  std::string server_name_copy(server_name);
+  SetConnectorOverride([=]() -> std::unique_ptr<wpi::NetworkStream> {
+    return wpi::TCPConnector::connect(server_name_copy.c_str(),
+                                      static_cast<int>(port),
+                                      Logger::GetInstance(), 1);
+  });
+}
+
+void Dispatcher::ClearServerOverride() { ClearConnectorOverride(); }
+
 Dispatcher::Dispatcher()
    : Dispatcher(Storage::GetInstance(), Notifier::GetInstance()) {}

@@ -98,18 +109,11 @@ void DispatcherBase::StartServer(
  m_clientserver_thread = std::thread(&Dispatcher::ServerThreadMain, this);
 }

-void DispatcherBase::StartClient(Connector connector) {
-  std::vector<Connector> connectors;
-  connectors.push_back(connector);
-  StartClient(std::move(connectors));
-}
-
-void DispatcherBase::StartClient(std::vector<Connector>&& connectors) {
+void DispatcherBase::StartClient() {
  {
    std::lock_guard<std::mutex> lock(m_user_mutex);
    if (m_active) return;
    m_active = true;
-    m_client_connectors = std::move(connectors);
  }
  m_server = false;
  using namespace std::placeholders;
@@ -195,6 +199,27 @@ void DispatcherBase::NotifyConnections(
  for (const auto& conn : m_connections) conn->NotifyIfActive(callback);
 }

+void DispatcherBase::SetConnector(Connector connector) {
+  std::vector<Connector> connectors;
+  connectors.push_back(connector);
+  SetConnector(std::move(connectors));
+}
+
+void DispatcherBase::SetConnector(std::vector<Connector>&& connectors) {
+  std::lock_guard<std::mutex> lock(m_user_mutex);
+  m_client_connectors = std::move(connectors);
+}
+
+void DispatcherBase::SetConnectorOverride(Connector connector) {
+  std::lock_guard<std::mutex> lock(m_user_mutex);
+  m_client_connector_override = std::move(connector);
+}
+
+void DispatcherBase::ClearConnectorOverride() {
+  std::lock_guard<std::mutex> lock(m_user_mutex);
+  m_client_connector_override = nullptr;
+}
+
 void DispatcherBase::DispatchThreadMain() {
  auto timeout_time = std::chrono::steady_clock::now();

@@ -320,9 +345,13 @@ void DispatcherBase::ClientThreadMain() {
    // get next server to connect to
    {
      std::lock_guard<std::mutex> lock(m_user_mutex);
-      if (m_client_connectors.empty()) continue;
-      if (i >= m_client_connectors.size()) i = 0;
-      connect = m_client_connectors[i++];
+      if (m_client_connector_override) {
+        connect = m_client_connector_override;
+      } else {
+        if (m_client_connectors.empty()) continue;
+        if (i >= m_client_connectors.size()) i = 0;
+        connect = m_client_connectors[i++];
+      }
    }

    // try to connect (with timeout)
--- a/src/Dispatcher.h
+++ b/src/Dispatcher.h
@@ -41,8 +41,7 @@ class DispatcherBase {

  void StartServer(llvm::StringRef persist_filename,
                   std::unique_ptr<wpi::NetworkAcceptor> acceptor);
-  void StartClient(Connector connector);
-  void StartClient(std::vector<Connector>&& connectors);
+  void StartClient();
  void Stop();
  void SetUpdateRate(double interval);
  void SetIdentity(llvm::StringRef name);
@@ -50,6 +49,12 @@ class DispatcherBase {
  std::vector<ConnectionInfo> GetConnections() const;
  void NotifyConnections(ConnectionListenerCallback callback) const;

+  void SetConnector(Connector connector);
+  void SetConnector(std::vector<Connector>&& connectors);
+
+  void SetConnectorOverride(Connector connector);
+  void ClearConnectorOverride();
+
  bool active() const { return m_active; }

  DispatcherBase(const DispatcherBase&) = delete;
@@ -85,6 +90,7 @@ class DispatcherBase {
  std::thread m_clientserver_thread;

  std::unique_ptr<wpi::NetworkAcceptor> m_server_acceptor;
+  Connector m_client_connector_override;
  std::vector<Connector> m_client_connectors;

  // Mutex for user-accessible items
@@ -118,8 +124,12 @@ class Dispatcher : public DispatcherBase {

  void StartServer(StringRef persist_filename, const char* listen_address,
                   unsigned int port);
-  void StartClient(const char* server_name, unsigned int port);
-  void StartClient(ArrayRef<std::pair<StringRef, unsigned int>> servers);
+
+  void SetServer(const char* server_name, unsigned int port);
+  void SetServer(ArrayRef<std::pair<StringRef, unsigned int>> servers);
+
+  void SetServerOverride(const char* server_name, unsigned int port);
+  void ClearServerOverride();

 private:
  Dispatcher();
--- a/src/DsClient.cpp
+++ b/src/DsClient.cpp
@@ -0,0 +1,150 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015. 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 "DsClient.h"
+
+#include "llvm/raw_ostream.h"
+#include "llvm/SmallString.h"
+#include "support/raw_socket_istream.h"
+#include "tcpsockets/TCPConnector.h"
+
+#include "Dispatcher.h"
+#include "Log.h"
+
+using namespace nt;
+
+ATOMIC_STATIC_INIT(DsClient)
+
+class DsClient::Thread : public wpi::SafeThread {
+ public:
+  Thread(unsigned int port) : m_port(port) {}
+
+  void Main();
+
+  unsigned int m_port;
+  std::unique_ptr<wpi::NetworkStream> m_stream;
+};
+
+void DsClient::Start(unsigned int port) {
+  auto thr = m_owner.GetThread();
+  if (!thr)
+    m_owner.Start(new Thread(port));
+  else
+    thr->m_port = port;
+}
+
+void DsClient::Stop() {
+  {
+    // Close the stream so the read (if any) terminates.
+    auto thr = m_owner.GetThread();
+    if (thr) {
+      thr->m_active = false;
+      if (thr->m_stream) thr->m_stream->close();
+    }
+  }
+  m_owner.Stop();
+}
+
+void DsClient::Thread::Main() {
+  unsigned int oldip = 0;
+  wpi::Logger nolog;  // to silence log messages from TCPConnector
+
+  while (m_active) {
+    // wait for periodic reconnect or termination
+    auto timeout_time =
+        std::chrono::steady_clock::now() + std::chrono::milliseconds(500);
+    unsigned int port;
+    {
+      std::unique_lock<std::mutex> lock(m_mutex);
+      m_cond.wait_until(lock, timeout_time, [&] { return !m_active; });
+      port = m_port;
+    }
+    if (!m_active) goto done;
+
+    // Try to connect to DS on the local machine
+    m_stream =
+        wpi::TCPConnector::connect("127.0.0.1", 1742, nolog, 1);
+    if (!m_active) goto done;
+    if (!m_stream) continue;
+
+    DEBUG3("connected to DS");
+    wpi::raw_socket_istream is(*m_stream);
+
+    while (m_active && !is.has_error()) {
+      // Read JSON "{...}".  This is very limited, does not handle quoted "}" or
+      // nested {}, but is sufficient for this purpose.
+      llvm::SmallString<128> json;
+      char ch;
+
+      // Throw away characters until {
+      do {
+        is.read(ch);
+        if (is.has_error()) break;
+        if (!m_active) goto done;
+      } while (ch != '{');
+      json += '{';
+
+      if (is.has_error()) {
+        m_stream = nullptr;
+        break;
+      }
+
+      // Read characters until }
+      do {
+        is.read(ch);
+        if (is.has_error()) break;
+        if (!m_active) goto done;
+        json += ch;
+      } while (ch != '}');
+
+      if (is.has_error()) {
+        m_stream = nullptr;
+        break;
+      }
+      DEBUG3("json=" << json);
+
+      // Look for "robotIP":12345, and get 12345 portion
+      size_t pos = json.find("\"robotIP\"");
+      if (pos == llvm::StringRef::npos) continue;  // could not find?
+      pos += 9;
+      pos = json.find(':', pos);
+      if (pos == llvm::StringRef::npos) continue;  // could not find?
+      size_t endpos = json.find_first_not_of("0123456789", pos + 1);
+      DEBUG3("found robotIP=" << json.slice(pos + 1, endpos));
+
+      // Parse into number
+      unsigned int ip;
+      if (json.slice(pos + 1, endpos).getAsInteger(10, ip)) continue;  // error
+
+      // If zero, clear the server override
+      if (ip == 0) {
+        Dispatcher::GetInstance().ClearServerOverride();
+        oldip = 0;
+        continue;
+      }
+
+      // If unchanged, don't reconnect
+      if (ip == oldip) continue;
+      oldip = ip;
+
+      // Convert number into dotted quad
+      json.clear();
+      llvm::raw_svector_ostream os{json};
+      os << ((ip >> 24) & 0xff) << "." << ((ip >> 16) & 0xff) << "."
+         << ((ip >> 8) & 0xff) << "." << (ip & 0xff);
+      INFO("client: DS overriding server IP to " << os.str());
+      Dispatcher::GetInstance().SetServerOverride(json.c_str(), port);
+    }
+
+    // We disconnected from the DS, clear the server override
+    Dispatcher::GetInstance().ClearServerOverride();
+    oldip = 0;
+  }
+
+done:
+  Dispatcher::GetInstance().ClearServerOverride();
+}
--- a/src/DsClient.h
+++ b/src/DsClient.h
@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#ifndef NT_DSCLIENT_H_
+#define NT_DSCLIENT_H_
+
+#include "support/atomic_static.h"
+#include "support/SafeThread.h"
+
+namespace nt {
+
+class DsClient {
+ public:
+  static DsClient& GetInstance() {
+    ATOMIC_STATIC(DsClient, instance);
+    return instance;
+  }
+  ~DsClient() = default;
+
+  void Start(unsigned int port);
+  void Stop();
+
+ private:
+  DsClient() = default;
+
+  class Thread;
+  wpi::SafeThreadOwner<Thread> m_owner;
+
+  ATOMIC_STATIC_DECL(DsClient)
+};
+
+}  // namespace nt
+
+#endif  // NT_DSCLIENT_H_
--- a/src/networktables/NetworkTable.cpp
+++ b/src/networktables/NetworkTable.cpp
@@ -2,6 +2,7 @@

 #include <algorithm>

+#include "llvm/raw_ostream.h"
 #include "llvm/SmallString.h"
 #include "llvm/StringMap.h"
 #include "tables/ITableListener.h"
@@ -11,20 +12,17 @@
 using llvm::StringRef;

 const char NetworkTable::PATH_SEPARATOR_CHAR = '/';
-std::vector<std::string> NetworkTable::s_ip_addresses;
 std::string NetworkTable::s_persistent_filename = "networktables.ini";
 bool NetworkTable::s_client = false;
+bool NetworkTable::s_enable_ds = true;
 bool NetworkTable::s_running = false;
 unsigned int NetworkTable::s_port = NT_DEFAULT_PORT;

 void NetworkTable::Initialize() {
  if (s_running) Shutdown();
  if (s_client) {
-    std::vector<std::pair<StringRef, unsigned int>> servers;
-    servers.reserve(s_ip_addresses.size());
-    for (const auto& ip_address : s_ip_addresses)
-      servers.emplace_back(std::make_pair(ip_address, s_port));
-    nt::StartClient(servers);
+    nt::StartClient();
+    if (s_enable_ds) nt::StartDSClient(s_port);
  } else
    nt::StartServer(s_persistent_filename, "", s_port);
  s_running = true;
@@ -32,9 +30,10 @@ void NetworkTable::Initialize() {

 void NetworkTable::Shutdown() {
  if (!s_running) return;
-  if (s_client)
+  if (s_client) {
+    nt::StopDSClient();
    nt::StopClient();
-  else
+  } else
    nt::StopServer();
  s_running = false;
 }
@@ -44,27 +43,74 @@ void NetworkTable::SetClientMode() { s_client = true; }
 void NetworkTable::SetServerMode() { s_client = false; }

 void NetworkTable::SetTeam(int team) {
-  char tmp[30];
-#ifdef _MSC_VER
-  sprintf_s(tmp, "roboRIO-%d-FRC.local", team);
-#else
-  using namespace std;
-  snprintf(tmp, 30, "roboRIO-%d-FRC.local", team);
-#endif
-  SetIPAddress(tmp);
+  std::pair<StringRef, unsigned int> servers[4];
+
+  // 10.te.am.2
+  llvm::SmallString<32> fixed;
+  {
+    llvm::raw_svector_ostream oss{fixed};
+    oss << "10." << static_cast<int>(team / 100) << '.'
+        << static_cast<int>(team % 100) << ".2";
+    servers[0] = std::make_pair(oss.str(), s_port);
+  }
+
+  // 172.22.11.2
+  servers[1] = std::make_pair("172.22.11.2", s_port);
+
+  // roboRIO-<team>-FRC.local
+  llvm::SmallString<32> mdns;
+  {
+    llvm::raw_svector_ostream oss{mdns};
+    oss << "roboRIO-" << team << "-FRC.local";
+    servers[2] = std::make_pair(oss.str(), s_port);
+  }
+
+  // roboRIO-<team>-FRC.lan
+  llvm::SmallString<32> mdns_lan;
+  {
+    llvm::raw_svector_ostream oss{mdns_lan};
+    oss << "roboRIO-" << team << "-FRC.lan";
+    servers[3] = std::make_pair(oss.str(), s_port);
+  }
+
+  nt::SetServer(servers);
 }

 void NetworkTable::SetIPAddress(StringRef address) {
-  s_ip_addresses.clear();
-  s_ip_addresses.emplace_back(address);
+  llvm::SmallString<32> addr_copy{address};
+  nt::SetServer(addr_copy.c_str(), s_port);
+
+  // Stop the DS client if we're explicitly connecting to localhost
+  if (address == "localhost" || address == "127.0.0.1")
+    nt::StopDSClient();
+  else if (s_enable_ds)
+    nt::StartDSClient(s_port);
 }

 void NetworkTable::SetIPAddress(llvm::ArrayRef<std::string> addresses) {
-  s_ip_addresses = addresses;
+  llvm::SmallVector<std::pair<StringRef, unsigned int>, 8> servers;
+  for (const auto& ip_address : addresses)
+    servers.emplace_back(std::make_pair(ip_address, s_port));
+  nt::SetServer(servers);
+
+  // Stop the DS client if we're explicitly connecting to localhost
+  if (!addresses.empty() &&
+      (addresses[0] == "localhost" || addresses[0] == "127.0.0.1"))
+    nt::StopDSClient();
+  else if (s_enable_ds)
+    nt::StartDSClient(s_port);
 }

 void NetworkTable::SetPort(unsigned int port) { s_port = port; }

+void NetworkTable::SetDSClientEnabled(bool enabled) {
+  s_enable_ds = enabled;
+  if (s_enable_ds)
+    nt::StartDSClient(s_port);
+  else
+    nt::StopDSClient();
+}
+
 void NetworkTable::SetPersistentFilename(StringRef filename) {
  s_persistent_filename = filename;
 }
--- a/src/ntcore_c.cpp
+++ b/src/ntcore_c.cpp
@@ -372,6 +372,8 @@ void NT_StartServer(const char* persist_filename, const char* listen_address,

 void NT_StopServer(void) { nt::StopServer(); }

+void NT_StartClientNone(void) { nt::StartClient(); }
+
 void NT_StartClient(const char* server_name, unsigned int port) {
  nt::StartClient(server_name, port);
 }
@@ -387,6 +389,23 @@ void NT_StartClientMulti(size_t count, const char** server_names,

 void NT_StopClient(void) { nt::StopClient(); }

+void NT_SetServer(const char* server_name, unsigned int port) {
+  nt::SetServer(server_name, port);
+}
+
+void NT_SetServerMulti(size_t count, const char** server_names,
+                         const unsigned int* ports) {
+  std::vector<std::pair<StringRef, unsigned int>> servers;
+  servers.reserve(count);
+  for (size_t i = 0; i < count; ++i)
+    servers.emplace_back(std::make_pair(server_names[i], ports[i]));
+  nt::SetServer(servers);
+}
+
+void NT_StartDSClient(unsigned int port) { nt::StartDSClient(port); }
+
+void NT_StopDSClient(void) { nt::StopDSClient(); }
+
 void NT_StopRpcServer(void) { nt::StopRpcServer(); }

 void NT_StopNotifier(void) { nt::StopNotifier(); }
--- a/src/ntcore_cpp.cpp
+++ b/src/ntcore_cpp.cpp
@@ -14,6 +14,7 @@
 #include "support/timestamp.h"
 #include "Log.h"
 #include "Dispatcher.h"
+#include "DsClient.h"
 #include "Notifier.h"
 #include "RpcServer.h"
 #include "Storage.h"
@@ -246,16 +247,34 @@ void StartServer(StringRef persist_filename, const char* listen_address,

 void StopServer() { Dispatcher::GetInstance().Stop(); }

+void StartClient() { Dispatcher::GetInstance().StartClient(); }
+
 void StartClient(const char* server_name, unsigned int port) {
-  Dispatcher::GetInstance().StartClient(server_name, port);
+  auto& d = Dispatcher::GetInstance();
+  d.SetServer(server_name, port);
+  d.StartClient();
 }

 void StartClient(ArrayRef<std::pair<StringRef, unsigned int>> servers) {
-  Dispatcher::GetInstance().StartClient(servers);
+  auto& d = Dispatcher::GetInstance();
+  d.SetServer(servers);
+  d.StartClient();
 }

 void StopClient() { Dispatcher::GetInstance().Stop(); }

+void SetServer(const char* server_name, unsigned int port) {
+  Dispatcher::GetInstance().SetServer(server_name, port);
+}
+
+void SetServer(ArrayRef<std::pair<StringRef, unsigned int>> servers) {
+  Dispatcher::GetInstance().SetServer(servers);
+}
+
+void StartDSClient(unsigned int port) { DsClient::GetInstance().Start(port); }
+
+void StopDSClient() { DsClient::GetInstance().Stop(); }
+
 void StopRpcServer() { RpcServer::GetInstance().Stop(); }

 void StopNotifier() { Notifier::GetInstance().Stop(); }