From 558151061e62a54da1080256619ec09c3a96f364 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Tue, 26 Oct 2021 23:34:27 -0700 Subject: [PATCH] [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/examples/dsclient/dsclient.cpp | 35 ++++++ wpiutil/src/main/native/cpp/DsClient.cpp | 107 ++++++++++++++++++ .../src/main/native/include/wpi/DsClient.h | 55 +++++++++ 3 files changed, 197 insertions(+) create mode 100644 wpiutil/examples/dsclient/dsclient.cpp create mode 100644 wpiutil/src/main/native/cpp/DsClient.cpp create mode 100644 wpiutil/src/main/native/include/wpi/DsClient.h diff --git a/wpiutil/examples/dsclient/dsclient.cpp b/wpiutil/examples/dsclient/dsclient.cpp new file mode 100644 index 0000000000..69c1061fdd --- /dev/null +++ b/wpiutil/examples/dsclient/dsclient.cpp @@ -0,0 +1,35 @@ +// 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 + +#include "fmt/format.h" +#include "wpi/DsClient.h" +#include "wpi/EventLoopRunner.h" +#include "wpi/Logger.h" +#include "wpi/uv/Error.h" + +namespace uv = wpi::uv; + +static void logfunc(unsigned int level, const char* file, unsigned int line, + const char* msg) { + std::fprintf(stderr, "(%d) %s:%d: %s\n", level, file, line, msg); +} + +int main() { + wpi::Logger logger{logfunc, 0}; + + // Kick off the event loop on a separate thread + wpi::EventLoopRunner loop; + std::shared_ptr client; + loop.ExecAsync([&](uv::Loop& loop) { + client = wpi::DsClient::Create(loop, logger); + client->setIp.connect( + [](std::string_view ip) { fmt::print("got IP: {}\n", ip); }); + client->clearIp.connect([] { std::fputs("cleared IP\n", stdout); }); + }); + + // wait for a keypress to terminate + std::getchar(); +} diff --git a/wpiutil/src/main/native/cpp/DsClient.cpp b/wpiutil/src/main/native/cpp/DsClient.cpp new file mode 100644 index 0000000000..455f10c2e8 --- /dev/null +++ b/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 +#include +#include +#include +#include + +#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(); + 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(); + } 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); + } +} diff --git a/wpiutil/src/main/native/include/wpi/DsClient.h b/wpiutil/src/main/native/include/wpi/DsClient.h new file mode 100644 index 0000000000..c24ba3b568 --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/DsClient.h @@ -0,0 +1,55 @@ +// 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. + +#pragma once + +#include +#include +#include + +#include "wpi/Signal.h" + +namespace wpi { + +class Logger; + +namespace uv { +class Loop; +class Tcp; +class Timer; +} // namespace uv + +class DsClient : public std::enable_shared_from_this { + struct private_init {}; + + public: + static std::shared_ptr Create(wpi::uv::Loop& loop, + wpi::Logger& logger) { + return std::make_shared(loop, logger, private_init{}); + } + + DsClient(wpi::uv::Loop& loop, wpi::Logger& logger, const private_init&); + ~DsClient(); + DsClient(const DsClient&) = delete; + DsClient& operator=(const DsClient&) = delete; + + void Close(); + + sig::Signal setIp; + sig::Signal<> clearIp; + + private: + void Connect(); + void HandleIncoming(std::string_view in); + void ParseJson(); + + wpi::Logger& m_logger; + + std::shared_ptr m_tcp; + std::shared_ptr m_timer; + + std::string m_json; +}; + +} // namespace wpi