diff --git a/hal/src/main/native/include/hal/simulation/SimCallbackRegistry.h b/hal/src/main/native/include/hal/simulation/SimCallbackRegistry.h index bdd6f45611..31f64eff45 100644 --- a/hal/src/main/native/include/hal/simulation/SimCallbackRegistry.h +++ b/hal/src/main/native/include/hal/simulation/SimCallbackRegistry.h @@ -30,7 +30,7 @@ class SimCallbackRegistryBase { public: void Cancel(int32_t uid) { std::scoped_lock lock(m_mutex); - if (m_callbacks) m_callbacks->erase(uid - 1); + if (m_callbacks && uid > 0) m_callbacks->erase(uid - 1); } void Reset() { diff --git a/settings.gradle b/settings.gradle index 897f4addfb..e020d0d9f2 100644 --- a/settings.gradle +++ b/settings.gradle @@ -32,6 +32,9 @@ include 'simulation:frc_gazebo_plugins' include 'simulation:halsim_gazebo' include 'simulation:halsim_ds_socket' include 'simulation:halsim_gui' +include 'simulation:halsim_ws_core' +include 'simulation:halsim_ws_client' +include 'simulation:halsim_ws_server' include 'cameraserver' include 'cameraserver:multiCameraServer' include 'wpilibOldCommands' diff --git a/simulation/CMakeLists.txt b/simulation/CMakeLists.txt index 5137f143eb..cb2b01c8b9 100644 --- a/simulation/CMakeLists.txt +++ b/simulation/CMakeLists.txt @@ -3,3 +3,6 @@ add_subdirectory(halsim_gui) #add_subdirectory(frc_gazebo_plugins) #add_subdirectory(halsim_gazebo) add_subdirectory(halsim_ds_socket) +add_subdirectory(halsim_ws_core) +add_subdirectory(halsim_ws_client) +add_subdirectory(halsim_ws_server) diff --git a/simulation/halsim_ws_client/CMakeLists.txt b/simulation/halsim_ws_client/CMakeLists.txt new file mode 100644 index 0000000000..d869f28eab --- /dev/null +++ b/simulation/halsim_ws_client/CMakeLists.txt @@ -0,0 +1,16 @@ +project(halsim_ws_client) + +include(CompileWarnings) + +file(GLOB halsim_ws_client_src src/main/native/cpp/*.cpp) + +add_library(halsim_ws_client MODULE ${halsim_ws_client_src}) +wpilib_target_warnings(halsim_ws_client) +set_target_properties(halsim_ws_client PROPERTIES DEBUG_POSTFIX "d") +target_link_libraries(halsim_ws_client PUBLIC hal halsim_ws_core) + +target_include_directories(halsim_ws_client PRIVATE src/main/native/include) + +set_property(TARGET halsim_ws_client PROPERTY FOLDER "libraries") + +install(TARGETS halsim_ws_client EXPORT halsim_ws_client DESTINATION "${main_lib_dest}") diff --git a/simulation/halsim_ws_client/build.gradle b/simulation/halsim_ws_client/build.gradle new file mode 100644 index 0000000000..c0137a4b60 --- /dev/null +++ b/simulation/halsim_ws_client/build.gradle @@ -0,0 +1,35 @@ +if (!project.hasProperty('onlylinuxathena')) { + + description = "WebSocket Client Extension" + + ext { + includeWpiutil = true + pluginName = 'halsim_ws_client' + } + + apply plugin: 'google-test-test-suite' + + + ext { + staticGtestConfigs = [:] + } + + staticGtestConfigs["${pluginName}Test"] = [] + apply from: "${rootDir}/shared/googletest.gradle" + + apply from: "${rootDir}/shared/plugins/setupBuild.gradle" + + model { + binaries { + all { + if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { + it.buildable = false + return + } + + lib project: ":simulation:halsim_ws_core", library: "halsim_ws_core", linkage: "static" + + } + } + } +} diff --git a/simulation/halsim_ws_client/src/dev/native/cpp/main.cpp b/simulation/halsim_ws_client/src/dev/native/cpp/main.cpp new file mode 100644 index 0000000000..1efcefc613 --- /dev/null +++ b/simulation/halsim_ws_client/src/dev/native/cpp/main.cpp @@ -0,0 +1,32 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include +#include +#include + +extern "C" int HALSIM_InitExtension(void); + +int main() { + HAL_Initialize(500, 0); + HALSIM_InitExtension(); + + HAL_RunMain(); + + int cycleCount = 0; + while (cycleCount < 100) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + cycleCount++; + std::cout << "Count: " << cycleCount << std::endl; + } + + std::cout << "DONE" << std::endl; + HAL_ExitMain(); +} diff --git a/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClient.cpp b/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClient.cpp new file mode 100644 index 0000000000..a5c3e85f95 --- /dev/null +++ b/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 +#include +#include + +#include "HALSimWSClientConnection.h" + +static constexpr int kTcpConnectAttemptTimeout = 1000; + +namespace uv = wpi::uv; + +namespace wpilibws { + +std::shared_ptr 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(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 hws) { + if (m_hws.lock()) { + return false; + } + + m_hws = hws; + + m_simDevicesProvider.OnNetworkConnected(hws); + + m_providers.ForEach([hws](std::shared_ptr provider) { + provider->OnNetworkConnected(hws); + }); + + return true; +} + +void HALSimWS::CloseWebsocket( + std::shared_ptr hws) { + // Inform the providers that they need to cancel callbacks + m_simDevicesProvider.OnNetworkDisconnected(); + + m_providers.ForEach([](std::shared_ptr 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(); + auto& device = msg.at("device").get_ref(); + + 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 diff --git a/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClientConnection.cpp b/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClientConnection.cpp new file mode 100644 index 0000000000..7a52691746 --- /dev/null +++ b/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 +#include + +#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(); + + 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 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 diff --git a/simulation/halsim_ws_client/src/main/native/cpp/main.cpp b/simulation/halsim_ws_client/src/main/native/cpp/main.cpp new file mode 100644 index 0000000000..ee0d16780c --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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(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" diff --git a/simulation/halsim_ws_client/src/main/native/include/HALSimWSClient.h b/simulation/halsim_ws_client/src/main/native/include/HALSimWSClient.h new file mode 100644 index 0000000000..24aa272eeb --- /dev/null +++ b/simulation/halsim_ws_client/src/main/native/include/HALSimWSClient.h @@ -0,0 +1,74 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace wpilibws { + +class HALSimWSClientConnection; + +class HALSimWS { + public: + static std::shared_ptr GetInstance() { return g_instance; } + static void SetInstance(std::shared_ptr inst) { g_instance = inst; } + + explicit HALSimWS(ProviderContainer& providers, + HALSimWSProviderSimDevices& simDevicesProvider) + : m_providers(providers), m_simDevicesProvider(simDevicesProvider) {} + HALSimWS(const HALSimWS&) = delete; + HALSimWS& operator=(const HALSimWS&) = delete; + + bool Initialize(); + static void Main(void*); + static void Exit(void*); + + bool RegisterWebsocket(std::shared_ptr hws); + void CloseWebsocket(std::shared_ptr hws); + + void OnNetValueChanged(const wpi::json& msg); + + std::string GetTargetHost() const { return m_host; } + std::string GetTargetUri() const { return m_uri; } + int GetTargetPort() { return m_port; } + std::shared_ptr GetLoop() { return m_loop; } + + private: + static std::shared_ptr g_instance; + + void MainLoop(); + + void AttemptConnect(); + + bool m_tcp_connected = false; + std::shared_ptr m_connect_timer; + int m_connect_attempts = 0; + + std::weak_ptr m_hws; + + ProviderContainer& m_providers; + HALSimWSProviderSimDevices& m_simDevicesProvider; + + std::shared_ptr m_loop; + std::shared_ptr m_tcp_client; + + std::string m_host; + std::string m_uri; + int m_port; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_client/src/main/native/include/HALSimWSClientConnection.h b/simulation/halsim_ws_client/src/main/native/include/HALSimWSClientConnection.h new file mode 100644 index 0000000000..f6d13432aa --- /dev/null +++ b/simulation/halsim_ws_client/src/main/native/include/HALSimWSClientConnection.h @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace wpilibws { + +class HALSimWS; + +class HALSimWSClientConnection + : public HALSimBaseWebSocketConnection, + public std::enable_shared_from_this { + public: + using BufferPool = wpi::uv::SimpleBufferPool<4>; + using LoopFunc = std::function; + using UvExecFunc = wpi::uv::AsyncFunction; + + explicit HALSimWSClientConnection(std::shared_ptr stream) + : m_stream(stream) {} + + public: + void OnSimValueChanged(const wpi::json& msg) override; + void Initialize(); + + private: + std::shared_ptr m_stream; + + bool m_ws_connected = false; + wpi::WebSocket* m_websocket = nullptr; + + std::shared_ptr m_exec; + std::unique_ptr m_buffers; + std::mutex m_buffers_mutex; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/CMakeLists.txt b/simulation/halsim_ws_core/CMakeLists.txt new file mode 100644 index 0000000000..56b176668d --- /dev/null +++ b/simulation/halsim_ws_core/CMakeLists.txt @@ -0,0 +1,16 @@ +project(halsim_ws_core) + +include(CompileWarnings) + +file(GLOB halsim_ws_core_src src/main/native/cpp/*.cpp) + +add_library(halsim_ws_core STATIC ${halsim_ws_core_src}) +wpilib_target_warnings(halsim_ws_core) +set_target_properties(halsim_ws_core PROPERTIES DEBUG_POSTFIX "d" POSITION_INDEPENDENT_CODE ON) +target_link_libraries(halsim_ws_core PUBLIC hal) + +target_include_directories(halsim_ws_core PUBLIC src/main/native/include) + +set_property(TARGET halsim_ws_core PROPERTY FOLDER "libraries") + +install(TARGETS halsim_ws_core EXPORT halsim_ws_core DESTINATION "${main_lib_dest}") diff --git a/simulation/halsim_ws_core/build.gradle b/simulation/halsim_ws_core/build.gradle new file mode 100644 index 0000000000..ccf02cee37 --- /dev/null +++ b/simulation/halsim_ws_core/build.gradle @@ -0,0 +1,58 @@ +apply plugin: 'cpp' +apply plugin: 'edu.wpi.first.NativeUtils' +apply plugin: ExtraTasks + +if (!project.hasProperty('onlylinuxathena')) { + + description = "Core library for WebSocket extensions" + + ext { + includeWpiutil = true + includeNtCore = true + pluginName = 'halsim_ws_core' + } + + apply plugin: 'google-test-test-suite' + + + ext { + staticGtestConfigs = [:] + } + + staticGtestConfigs["${pluginName}Test"] = [] + apply from: "${rootDir}/shared/googletest.gradle" + + apply from: "${rootDir}/shared/config.gradle" + + model { + components { + halsim_ws_core(NativeLibrarySpec) { + sources { + cpp { + source { + srcDirs = ['src/main/native/cpp'] + includes = ["**/*.cpp"] + } + exportedHeaders { + srcDirs = ["src/main/native/include"] + } + } + } + binaries.all { + project(':hal').addHalDependency(it, 'shared') + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' + + } + appendDebugPathToBinaries(binaries) + } + } + binaries { + all { + if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { + it.buildable = false + return + } + } + } + } +} diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSBaseProvider.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSBaseProvider.cpp new file mode 100644 index 0000000000..39057a1930 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/cpp/WSBaseProvider.cpp @@ -0,0 +1,20 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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 "WSBaseProvider.h" + +namespace wpilibws { + +HALSimWSBaseProvider::HALSimWSBaseProvider(const std::string& key, + const std::string& type) + : m_key(key), m_type(type) {} + +void HALSimWSBaseProvider::OnNetValueChanged(const wpi::json& json) { + // empty +} + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSHalProviders.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSHalProviders.cpp new file mode 100644 index 0000000000..bc782023da --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/cpp/WSHalProviders.cpp @@ -0,0 +1,40 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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 "WSHalProviders.h" + +namespace wpilibws { + +void HALSimWSHalProvider::OnNetworkConnected( + std::shared_ptr ws) { + { + // store a weak reference to the websocket object + m_ws = ws; + } + + RegisterCallbacks(); +} + +void HALSimWSHalProvider::OnNetworkDisconnected() { CancelCallbacks(); } + +void HALSimWSHalProvider::ProcessHalCallback(const wpi::json& payload) { + auto ws = m_ws.lock(); + if (ws) { + wpi::json netValue = { + {"type", m_type}, {"device", m_deviceId}, {"data", payload}}; + ws->OnSimValueChanged(netValue); + } +} + +HALSimWSHalChanProvider::HALSimWSHalChanProvider(int32_t channel, + const std::string& key, + const std::string& type) + : HALSimWSHalProvider(key, type), m_channel(channel) { + m_deviceId = std::to_string(channel); +} + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Analog.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Analog.cpp new file mode 100644 index 0000000000..280ce5e4f1 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Analog.cpp @@ -0,0 +1,128 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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 "WSProvider_Analog.h" + +#include +#include +#include + +#define REGISTER_AIN(halsim, jsonid, ctype, haltype) \ + HALSIM_RegisterAnalogIn##halsim##Callback( \ + m_channel, \ + [](const char* name, void* param, const struct HAL_Value* value) { \ + static_cast(param)->ProcessHalCallback( \ + {{jsonid, static_cast(value->data.v_##haltype)}}); \ + }, \ + this, true) + +#define REGISTER_AIN_ACCUM(halsim, jsonid, ctype, haltype) \ + HALSIM_RegisterAnalogInAccumulator##halsim##Callback( \ + m_channel, \ + [](const char* name, void* param, const struct HAL_Value* value) { \ + static_cast(param)->ProcessHalCallback( \ + {{jsonid, static_cast(value->data.v_##haltype)}}); \ + }, \ + this, true) + +#define REGISTER_AOUT(halsim, jsonid, ctype, haltype) \ + HALSIM_RegisterAnalogOut##halsim##Callback( \ + m_channel, \ + [](const char* name, void* param, const struct HAL_Value* value) { \ + static_cast(param)->ProcessHalCallback( \ + {{jsonid, static_cast(value->data.v_##haltype)}}); \ + }, \ + this, true) + +namespace wpilibws { + +void HALSimWSProviderAnalogIn::Initialize(WSRegisterFunc webRegisterFunc) { + CreateProviders("AI", HAL_GetNumAnalogInputs(), + webRegisterFunc); +} + +HALSimWSProviderAnalogIn::~HALSimWSProviderAnalogIn() { CancelCallbacks(); } + +void HALSimWSProviderAnalogIn::RegisterCallbacks() { + m_initCbKey = REGISTER_AIN(Initialized, "voltage", double, double); + + m_accumInitCbKey = + REGISTER_AIN_ACCUM(Initialized, "accum_value", int64_t, + long); // NOLINT(runtime/int) + m_accumCountCbKey = REGISTER_AIN_ACCUM(Count, ">accum_count", int64_t, + long); // NOLINT(runtime/int) + m_accumCenterCbKey = + REGISTER_AIN_ACCUM(Center, "voltage")) != json.end()) { + HALSIM_SetAnalogInVoltage(m_channel, it.value()); + } + if ((it = json.find(">accum_value")) != json.end()) { + HALSIM_SetAnalogInAccumulatorValue(m_channel, it.value()); + } + if ((it = json.find(">accum_count")) != json.end()) { + HALSIM_SetAnalogInAccumulatorCount(m_channel, it.value()); + } +} + +void HALSimWSProviderAnalogOut::Initialize(WSRegisterFunc webRegisterFunc) { + CreateProviders("AO", HAL_GetNumAnalogOutputs(), + webRegisterFunc); +} + +HALSimWSProviderAnalogOut::~HALSimWSProviderAnalogOut() { CancelCallbacks(); } + +void HALSimWSProviderAnalogOut::RegisterCallbacks() { + m_initCbKey = REGISTER_AOUT(Initialized, " +#include + +#define REGISTER(halsim, jsonid, ctype, haltype) \ + HALSIM_RegisterDIO##halsim##Callback( \ + m_channel, \ + [](const char* name, void* param, const struct HAL_Value* value) { \ + static_cast(param)->ProcessHalCallback( \ + {{jsonid, static_cast(value->data.v_##haltype)}}); \ + }, \ + this, true) + +namespace wpilibws { + +void HALSimWSProviderDIO::Initialize(WSRegisterFunc webRegisterFunc) { + CreateProviders("DIO", HAL_GetNumDigitalChannels(), + webRegisterFunc); +} + +HALSimWSProviderDIO::~HALSimWSProviderDIO() { CancelCallbacks(); } + +void HALSimWSProviderDIO::RegisterCallbacks() { + m_initCbKey = REGISTER(Initialized, "value", bool, boolean); + m_pulseLengthCbKey = REGISTER(PulseLength, "value")) != json.end()) { + HALSIM_SetDIOValue(m_channel, static_cast(it.value())); + } +} + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp new file mode 100644 index 0000000000..93aafdf91c --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp @@ -0,0 +1,150 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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 "WSProvider_DriverStation.h" + +#include + +#include +#include +#include +#include + +#define REGISTER(halsim, jsonid, ctype, haltype) \ + HALSIM_RegisterDriverStation##halsim##Callback( \ + [](const char* name, void* param, const struct HAL_Value* value) { \ + static_cast(param) \ + ->ProcessHalCallback( \ + {{jsonid, static_cast(value->data.v_##haltype)}}); \ + }, \ + this, true) + +namespace wpilibws { + +void HALSimWSProviderDriverStation::Initialize(WSRegisterFunc webRegisterFunc) { + CreateSingleProvider("DriverStation", + webRegisterFunc); +} + +HALSimWSProviderDriverStation::~HALSimWSProviderDriverStation() { + CancelCallbacks(); +} + +void HALSimWSProviderDriverStation::RegisterCallbacks() { + m_enabledCbKey = REGISTER(Enabled, ">enabled", bool, boolean); + m_autonomousCbKey = REGISTER(Autonomous, ">autonomous", bool, boolean); + m_testCbKey = REGISTER(Test, ">test", bool, boolean); + m_estopCbKey = REGISTER(EStop, ">estop", bool, boolean); + m_fmsCbKey = REGISTER(FmsAttached, ">fms", bool, boolean); + m_dsCbKey = REGISTER(DsAttached, ">ds", bool, boolean); + + // Special case for new data, since the HAL_Value is empty + m_newDataCbKey = HALSIM_RegisterDriverStationNewDataCallback( + [](const char* name, void* param, const struct HAL_Value* value) { + static_cast(param)->ProcessHalCallback( + {{">new_data", true}}); + }, + this, true); + + m_allianceCbKey = HALSIM_RegisterDriverStationAllianceStationIdCallback( + [](const char* name, void* param, const struct HAL_Value* value) { + std::string station; + switch (static_cast(value->data.v_enum)) { + case HAL_AllianceStationID_kRed1: + station = "red1"; + break; + case HAL_AllianceStationID_kBlue1: + station = "blue1"; + break; + case HAL_AllianceStationID_kRed2: + station = "red2"; + break; + case HAL_AllianceStationID_kBlue2: + station = "blue2"; + break; + case HAL_AllianceStationID_kRed3: + station = "red3"; + break; + case HAL_AllianceStationID_kBlue3: + station = "blue3"; + break; + } + static_cast(param)->ProcessHalCallback( + {{">station", station}}); + }, + this, true); + + m_matchTimeCbKey = REGISTER(MatchTime, "enabled")) != json.end()) { + HALSIM_SetDriverStationEnabled(it.value()); + } + if ((it = json.find(">autonomous")) != json.end()) { + HALSIM_SetDriverStationAutonomous(it.value()); + } + if ((it = json.find(">test")) != json.end()) { + HALSIM_SetDriverStationTest(it.value()); + } + if ((it = json.find(">estop")) != json.end()) { + HALSIM_SetDriverStationEStop(it.value()); + } + if ((it = json.find(">fms")) != json.end()) { + HALSIM_SetDriverStationFmsAttached(it.value()); + } + if ((it = json.find(">ds")) != json.end()) { + HALSIM_SetDriverStationDsAttached(it.value()); + } + + if ((it = json.find(">station")) != json.end()) { + auto& station = it.value().get_ref(); + if (station == "red1") { + HALSIM_SetDriverStationAllianceStationId(HAL_AllianceStationID_kRed1); + } else if (station == "red2") { + HALSIM_SetDriverStationAllianceStationId(HAL_AllianceStationID_kRed2); + } else if (station == "red3") { + HALSIM_SetDriverStationAllianceStationId(HAL_AllianceStationID_kRed3); + } else if (station == "blue1") { + HALSIM_SetDriverStationAllianceStationId(HAL_AllianceStationID_kBlue1); + } else if (station == "blue2") { + HALSIM_SetDriverStationAllianceStationId(HAL_AllianceStationID_kBlue2); + } else if (station == "blue3") { + HALSIM_SetDriverStationAllianceStationId(HAL_AllianceStationID_kBlue3); + } + } + + // Only notify usercode if we get the new data message + if ((it = json.find(">new_data")) != json.end()) { + HALSIM_NotifyDriverStationNewData(); + } +} + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp new file mode 100644 index 0000000000..1a529f2a53 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp @@ -0,0 +1,87 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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 "WSProvider_Encoder.h" + +#include +#include + +#define REGISTER(halsim, jsonid, ctype, haltype) \ + HALSIM_RegisterEncoder##halsim##Callback( \ + m_channel, \ + [](const char* name, void* param, const struct HAL_Value* value) { \ + static_cast(param)->ProcessHalCallback( \ + {{jsonid, static_cast(value->data.v_##haltype)}}); \ + }, \ + this, true) + +namespace wpilibws { + +void HALSimWSProviderEncoder::Initialize(WSRegisterFunc webRegisterFunc) { + CreateProviders("Encoder", HAL_GetNumEncoders(), + webRegisterFunc); +} + +HALSimWSProviderEncoder::~HALSimWSProviderEncoder() { CancelCallbacks(); } + +void HALSimWSProviderEncoder::RegisterCallbacks() { + // Special case for initialization since we will need to send + // the digital channels down the line as well + m_initCbKey = HALSIM_RegisterEncoderInitializedCallback( + m_channel, + [](const char* name, void* param, const struct HAL_Value* value) { + auto provider = static_cast(param); + bool init = static_cast(value->data.v_boolean); + + wpi::json payload = {{"GetChannel()); + payload["GetChannel()); + } + + provider->ProcessHalCallback(payload); + }, + this, true); + m_countCbKey = REGISTER(Count, ">count", int32_t, int); + m_periodCbKey = REGISTER(Period, ">period", double, double); + m_resetCbKey = REGISTER(Reset, "count")) != json.end()) { + HALSIM_SetEncoderCount(m_channel, it.value()); + } + if ((it = json.find(">period")) != json.end()) { + HALSIM_SetEncoderPeriod(m_channel, it.value()); + } +} + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Joystick.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Joystick.cpp new file mode 100644 index 0000000000..b1f17a2f2c --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Joystick.cpp @@ -0,0 +1,110 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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 "WSProvider_Joystick.h" + +#include +#include + +namespace wpilibws { + +void HALSimWSProviderJoystick::Initialize(WSRegisterFunc webregisterFunc) { + CreateProviders("Joystick", HAL_kMaxJoysticks, + webregisterFunc); +} + +HALSimWSProviderJoystick::~HALSimWSProviderJoystick() { CancelCallbacks(); } + +void HALSimWSProviderJoystick::RegisterCallbacks() { + m_dsNewDataCbKey = HALSIM_RegisterDriverStationNewDataCallback( + [](const char* name, void* param, const struct HAL_Value* value) { + auto provider = static_cast(param); + + // Grab all joystick data and send it at once + wpi::json payload; + + // Axes data + HAL_JoystickAxes axes{}; + std::vector axesValues; + HALSIM_GetJoystickAxes(provider->GetChannel(), &axes); + + for (int i = 0; i < axes.count; i++) { + axesValues.push_back(axes.axes[i]); + } + + // POVs data + HAL_JoystickPOVs povs{}; + std::vector povsValues; + HALSIM_GetJoystickPOVs(provider->GetChannel(), &povs); + + for (int i = 0; i < povs.count; i++) { + povsValues.push_back(povs.povs[i]); + } + + // Button data + HAL_JoystickButtons buttons{}; + std::vector buttonsValues; + + for (int i = 0; i < buttons.count; i++) { + buttonsValues.push_back(((buttons.buttons >> i) & 0x1) == 1); + } + + payload[">axes"] = axesValues; + payload[">povs"] = povsValues; + payload[">buttons"] = buttonsValues; + + provider->ProcessHalCallback(payload); + }, + this, true); +} + +void HALSimWSProviderJoystick::CancelCallbacks() { + HALSIM_CancelDriverStationNewDataCallback(m_dsNewDataCbKey); + + m_dsNewDataCbKey = 0; +} + +void HALSimWSProviderJoystick::OnNetValueChanged(const wpi::json& json) { + wpi::json::const_iterator it; + if ((it = json.find(">axes")) != json.end()) { + HAL_JoystickAxes axes{}; + axes.count = + std::min(it.value().size(), (wpi::json::size_type)HAL_kMaxJoystickAxes); + for (int i = 0; i < axes.count; i++) { + axes.axes[i] = it.value()[i]; + } + + HALSIM_SetJoystickAxes(m_channel, &axes); + } + + if ((it = json.find(">buttons")) != json.end()) { + HAL_JoystickButtons buttons{}; + buttons.count = std::min(it.value().size(), (wpi::json::size_type)32); + for (int i = 0; i < buttons.count; i++) { + if (it.value()[i]) { + buttons.buttons |= 1 << (i - 1); + } + } + + HALSIM_SetJoystickButtons(m_channel, &buttons); + } + + if ((it = json.find(">povs")) != json.end()) { + HAL_JoystickPOVs povs{}; + povs.count = + std::min(it.value().size(), (wpi::json::size_type)HAL_kMaxJoystickPOVs); + for (int i = 0; i < povs.count; i++) { + povs.povs[i] = it.value()[i]; + } + + HALSIM_SetJoystickPOVs(m_channel, &povs); + } + // We won't send any updates to user code until the new data message + // is received by the driver station provider +} + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PWM.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PWM.cpp new file mode 100644 index 0000000000..8b790c1e02 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PWM.cpp @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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 "WSProvider_PWM.h" + +#include +#include + +#define REGISTER(halsim, jsonid, ctype, haltype) \ + HALSIM_RegisterPWM##halsim##Callback( \ + m_channel, \ + [](const char* name, void* param, const struct HAL_Value* value) { \ + static_cast(param)->ProcessHalCallback( \ + {{jsonid, static_cast(value->data.v_##haltype)}}); \ + }, \ + this, true) +namespace wpilibws { + +void HALSimWSProviderPWM::Initialize(WSRegisterFunc webRegisterFunc) { + CreateProviders("PWM", HAL_GetNumPWMChannels(), + webRegisterFunc); +} + +HALSimWSProviderPWM::~HALSimWSProviderPWM() { CancelCallbacks(); } + +void HALSimWSProviderPWM::RegisterCallbacks() { + m_initCbKey = REGISTER(Initialized, " +#include + +#define REGISTER(halsim, jsonid, ctype, haltype) \ + HALSIM_RegisterRelay##halsim##Callback( \ + m_channel, \ + [](const char* name, void* param, const struct HAL_Value* value) { \ + static_cast(param)->ProcessHalCallback( \ + {{jsonid, static_cast(value->data.v_##haltype)}}); \ + }, \ + this, true) + +namespace wpilibws { +void HALSimWSProviderRelay::Initialize(WSRegisterFunc webRegisterFunc) { + CreateProviders("Relay", HAL_GetNumRelayHeaders(), + webRegisterFunc); +} + +HALSimWSProviderRelay::~HALSimWSProviderRelay() { CancelCallbacks(); } + +void HALSimWSProviderRelay::RegisterCallbacks() { + m_initFwdCbKey = REGISTER(InitializedForward, " +#include + +#define REGISTER(halsim, jsonid, ctype, haltype) \ + HALSIM_RegisterRoboRio##halsim##Callback( \ + [](const char* name, void* param, const struct HAL_Value* value) { \ + static_cast(param)->ProcessHalCallback( \ + {{jsonid, static_cast(value->data.v_##haltype)}}); \ + }, \ + this, true) + +namespace wpilibws { + +void HALSimWSProviderRoboRIO::Initialize(WSRegisterFunc webRegisterFunc) { + CreateSingleProvider("RoboRIO", webRegisterFunc); +} + +HALSimWSProviderRoboRIO::~HALSimWSProviderRoboRIO() { CancelCallbacks(); } + +void HALSimWSProviderRoboRIO::RegisterCallbacks() { + m_fpgaCbKey = REGISTER(FPGAButton, ">fpga_button", bool, boolean); + m_vinVoltageCbKey = REGISTER(VInVoltage, ">vin_voltage", double, double); + m_vinCurrentCbKey = REGISTER(VInCurrent, ">vin_current", double, double); + + m_6vVoltageCbKey = REGISTER(UserVoltage6V, ">6v_voltage", double, double); + m_6vCurrentCbKey = REGISTER(UserCurrent6V, ">6v_current", double, double); + m_6vActiveCbKey = REGISTER(UserActive6V, ">6v_active", bool, boolean); + m_6vFaultsCbKey = REGISTER(UserFaults6V, ">6v_faults", int32_t, int); + + m_5vVoltageCbKey = REGISTER(UserVoltage5V, ">5v_voltage", double, double); + m_5vCurrentCbKey = REGISTER(UserCurrent5V, ">5v_current", double, double); + m_5vActiveCbKey = REGISTER(UserActive5V, ">5v_active", bool, boolean); + m_5vFaultsCbKey = REGISTER(UserFaults5V, ">5v_faults", int32_t, int); + + m_3v3VoltageCbKey = REGISTER(UserVoltage3V3, ">3v3_voltage", double, double); + m_3v3CurrentCbKey = REGISTER(UserCurrent3V3, ">3v3_current", double, double); + m_3v3ActiveCbKey = REGISTER(UserActive3V3, ">3v3_active", bool, boolean); + m_3v3FaultsCbKey = REGISTER(UserFaults3V3, ">3v3_faults", int32_t, int); +} + +void HALSimWSProviderRoboRIO::CancelCallbacks() { + HALSIM_CancelRoboRioFPGAButtonCallback(m_fpgaCbKey); + HALSIM_CancelRoboRioVInVoltageCallback(m_vinVoltageCbKey); + HALSIM_CancelRoboRioVInCurrentCallback(m_vinCurrentCbKey); + + HALSIM_CancelRoboRioUserVoltage6VCallback(m_6vVoltageCbKey); + HALSIM_CancelRoboRioUserCurrent6VCallback(m_6vCurrentCbKey); + HALSIM_CancelRoboRioUserActive6VCallback(m_6vActiveCbKey); + HALSIM_CancelRoboRioUserFaults6VCallback(m_6vFaultsCbKey); + + HALSIM_CancelRoboRioUserVoltage5VCallback(m_5vVoltageCbKey); + HALSIM_CancelRoboRioUserCurrent5VCallback(m_5vCurrentCbKey); + HALSIM_CancelRoboRioUserActive5VCallback(m_5vActiveCbKey); + HALSIM_CancelRoboRioUserFaults5VCallback(m_5vFaultsCbKey); + + HALSIM_CancelRoboRioUserVoltage3V3Callback(m_3v3VoltageCbKey); + HALSIM_CancelRoboRioUserCurrent3V3Callback(m_3v3CurrentCbKey); + HALSIM_CancelRoboRioUserActive3V3Callback(m_3v3ActiveCbKey); + HALSIM_CancelRoboRioUserFaults3V3Callback(m_3v3FaultsCbKey); + + m_fpgaCbKey = 0; + m_vinVoltageCbKey = 0; + m_vinCurrentCbKey = 0; + m_6vActiveCbKey = 0; + m_6vCurrentCbKey = 0; + m_6vFaultsCbKey = 0; + m_6vVoltageCbKey = 0; + m_5vActiveCbKey = 0; + m_5vCurrentCbKey = 0; + m_5vFaultsCbKey = 0; + m_5vVoltageCbKey = 0; + m_3v3ActiveCbKey = 0; + m_3v3CurrentCbKey = 0; + m_3v3FaultsCbKey = 0; + m_3v3VoltageCbKey = 0; +} + +void HALSimWSProviderRoboRIO::OnNetValueChanged(const wpi::json& json) { + wpi::json::const_iterator it; + if ((it = json.find(">fpga_button")) != json.end()) { + HALSIM_SetRoboRioFPGAButton(static_cast(it.value())); + } + + if ((it = json.find(">vin_voltage")) != json.end()) { + HALSIM_SetRoboRioVInVoltage(it.value()); + } + if ((it = json.find(">vin_current")) != json.end()) { + HALSIM_SetRoboRioVInCurrent(it.value()); + } + + if ((it = json.find(">6v_voltage")) != json.end()) { + HALSIM_SetRoboRioUserVoltage6V(it.value()); + } + if ((it = json.find(">6v_current")) != json.end()) { + HALSIM_SetRoboRioUserCurrent6V(it.value()); + } + if ((it = json.find(">6v_active")) != json.end()) { + HALSIM_SetRoboRioUserActive6V(static_cast(it.value())); + } + if ((it = json.find(">6v_faults")) != json.end()) { + HALSIM_SetRoboRioUserFaults6V(it.value()); + } + + if ((it = json.find(">5v_voltage")) != json.end()) { + HALSIM_SetRoboRioUserVoltage5V(it.value()); + } + if ((it = json.find(">5v_current")) != json.end()) { + HALSIM_SetRoboRioUserCurrent5V(it.value()); + } + if ((it = json.find(">5v_active")) != json.end()) { + HALSIM_SetRoboRioUserActive5V(static_cast(it.value())); + } + if ((it = json.find(">5v_faults")) != json.end()) { + HALSIM_SetRoboRioUserFaults5V(it.value()); + } + + if ((it = json.find(">3v3_voltage")) != json.end()) { + HALSIM_SetRoboRioUserVoltage3V3(it.value()); + } + if ((it = json.find(">3v3_current")) != json.end()) { + HALSIM_SetRoboRioUserCurrent3V3(it.value()); + } + if ((it = json.find(">3v3_active")) != json.end()) { + HALSIM_SetRoboRioUserActive3V3(static_cast(it.value())); + } + if ((it = json.find(">3v3_faults")) != json.end()) { + HALSIM_SetRoboRioUserFaults3V3(it.value()); + } +} + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_SimDevice.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_SimDevice.cpp new file mode 100644 index 0000000000..df054725f3 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_SimDevice.cpp @@ -0,0 +1,197 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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 "WSProvider_SimDevice.h" + +#include + +namespace wpilibws { + +HALSimWSProviderSimDevice::~HALSimWSProviderSimDevice() { CancelCallbacks(); } + +void HALSimWSProviderSimDevice::OnNetworkConnected( + std::shared_ptr ws) { + auto storedWS = m_ws.lock(); + + if (ws == storedWS) { + return; + } + + // Otherwise, run the disconnection code first so we get + // back to a clean slate (only if the stored websocket is + // not null) + if (storedWS) { + OnNetworkDisconnected(); + } + + m_ws = ws; + + m_simValueCreatedCbKey = HALSIM_RegisterSimValueCreatedCallback( + m_handle, this, HALSimWSProviderSimDevice::OnValueCreatedStatic, 1); +} + +void HALSimWSProviderSimDevice::OnNetworkDisconnected() { + // Cancel all callbacks + CancelCallbacks(); + + m_ws.reset(); +} + +void HALSimWSProviderSimDevice::CancelCallbacks() { + HALSIM_CancelSimValueCreatedCallback(m_simValueCreatedCbKey); + + m_simValueCreatedCbKey = 0; + + for (auto& kv : m_simValueChangedCbKeys) { + HALSIM_CancelSimValueChangedCallback(kv.getValue()); + } + + m_simValueChangedCbKeys.clear(); +} + +void HALSimWSProviderSimDevice::OnNetValueChanged(const wpi::json& json) { + auto it = json.cbegin(); + auto end = json.cend(); + + std::shared_lock lock(m_vhLock); + for (; it != end; ++it) { + auto vd = m_valueHandles.find(it.key()); + if (vd != m_valueHandles.end()) { + HAL_Value value; + value.type = vd->second->valueType; + switch (value.type) { + case HAL_BOOLEAN: + value.data.v_boolean = static_cast(it.value()) ? 1 : 0; + break; + case HAL_DOUBLE: + value.data.v_double = it.value(); + break; + case HAL_ENUM: + value.data.v_enum = it.value(); + break; + case HAL_INT: + value.data.v_int = it.value(); + break; + case HAL_LONG: + value.data.v_long = it.value(); + break; + default: + break; + } + + HAL_SetSimValue(vd->second->handle, &value); + } + } +} + +void HALSimWSProviderSimDevice::OnValueCreated(const char* name, + HAL_SimValueHandle handle, + HAL_Bool readonly, + const struct HAL_Value* value) { + wpi::Twine key = wpi::Twine(readonly ? "<" : "<>") + name; + auto data = std::make_unique(); + data->device = this; + data->handle = handle; + data->key = key.str(); + data->valueType = value->type; + + auto param = data.get(); + + { + std::unique_lock lock(m_vhLock); + m_valueHandles[data->key] = std::move(data); + } + + int32_t cbKey = HALSIM_RegisterSimValueChangedCallback( + handle, param, HALSimWSProviderSimDevice::OnValueChangedStatic, true); + + m_simValueChangedCbKeys[key.str()] = cbKey; +} + +void HALSimWSProviderSimDevice::OnValueChanged(SimDeviceValueData* valueData, + const struct HAL_Value* value) { + auto ws = m_ws.lock(); + if (ws) { + switch (value->type) { + case HAL_BOOLEAN: + ProcessHalCallback({{valueData->key, value->data.v_boolean}}); + break; + case HAL_DOUBLE: + ProcessHalCallback({{valueData->key, value->data.v_double}}); + break; + case HAL_ENUM: + ProcessHalCallback({{valueData->key, value->data.v_enum}}); + break; + case HAL_INT: + ProcessHalCallback({{valueData->key, value->data.v_int}}); + break; + case HAL_LONG: + ProcessHalCallback({{valueData->key, value->data.v_long}}); + break; + default: + break; + } + } +} + +void HALSimWSProviderSimDevice::ProcessHalCallback(const wpi::json& payload) { + auto ws = m_ws.lock(); + if (ws) { + wpi::json netValue = { + {"type", "SimDevices"}, {"device", m_deviceId}, {"data", payload}}; + ws->OnSimValueChanged(netValue); + } +} + +HALSimWSProviderSimDevices::~HALSimWSProviderSimDevices() { CancelCallbacks(); } + +void HALSimWSProviderSimDevices::DeviceCreatedCallback( + const char* name, HAL_SimDeviceHandle handle) { + auto key = (wpi::Twine("SimDevices/") + name).str(); + auto dev = std::make_shared( + handle, key, wpi::Twine(name).str()); + m_providers.Add(key, dev); + + if (m_ws) { + m_exec->Call([this, dev]() { dev->OnNetworkConnected(GetWSConnection()); }); + } +} + +void HALSimWSProviderSimDevices::DeviceFreedCallback( + const char* name, HAL_SimDeviceHandle handle) { + m_providers.Delete(name); +} + +void HALSimWSProviderSimDevices::Initialize( + std::shared_ptr loop) { + m_deviceCreatedCbKey = HALSIM_RegisterSimDeviceCreatedCallback( + "", this, HALSimWSProviderSimDevices::DeviceCreatedCallbackStatic, 1); + m_deviceFreedCbKey = HALSIM_RegisterSimDeviceFreedCallback( + "", this, HALSimWSProviderSimDevices::DeviceFreedCallbackStatic); + + m_exec = UvExecFn::Create(loop, [](auto out, LoopFn func) { + func(); + out.set_value(); + }); +} + +void HALSimWSProviderSimDevices::CancelCallbacks() { + HALSIM_CancelSimDeviceCreatedCallback(m_deviceCreatedCbKey); + HALSIM_CancelSimDeviceFreedCallback(m_deviceFreedCbKey); + + m_deviceCreatedCbKey = 0; + m_deviceFreedCbKey = 0; +} + +void HALSimWSProviderSimDevices::OnNetworkConnected( + std::shared_ptr hws) { + m_ws = hws; +} + +void HALSimWSProviderSimDevices::OnNetworkDisconnected() { m_ws = nullptr; } + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_dPWM.cpp b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_dPWM.cpp new file mode 100644 index 0000000000..881bb28f97 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_dPWM.cpp @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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 "WSProvider_dPWM.h" + +#include +#include + +#define REGISTER(halsim, jsonid, ctype, haltype) \ + HALSIM_RegisterDigitalPWM##halsim##Callback( \ + m_channel, \ + [](const char* name, void* param, const struct HAL_Value* value) { \ + static_cast(param)->ProcessHalCallback( \ + {{jsonid, static_cast(value->data.v_##haltype)}}); \ + }, \ + this, true) + +namespace wpilibws { + +void HALSimWSProviderDigitalPWM::Initialize(WSRegisterFunc webRegisterFunc) { + CreateProviders( + "dPWM", HAL_GetNumDigitalPWMOutputs(), webRegisterFunc); +} + +HALSimWSProviderDigitalPWM::~HALSimWSProviderDigitalPWM() { CancelCallbacks(); } + +void HALSimWSProviderDigitalPWM::RegisterCallbacks() { + m_initCbKey = REGISTER(Initialized, " + +#include + +namespace wpilibws { + +class HALSimBaseWebSocketConnection { + public: + virtual void OnSimValueChanged(const wpi::json& msg) = 0; + + protected: + virtual ~HALSimBaseWebSocketConnection() = default; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSBaseProvider.h b/simulation/halsim_ws_core/src/main/native/include/WSBaseProvider.h new file mode 100644 index 0000000000..46873b7abf --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSBaseProvider.h @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include + +#include "HALSimBaseWebSocketConnection.h" + +namespace wpilibws { + +class HALSimWSBaseProvider { + public: + explicit HALSimWSBaseProvider(const std::string& key, + const std::string& type = ""); + virtual ~HALSimWSBaseProvider() {} + + HALSimWSBaseProvider(const HALSimWSBaseProvider&) = delete; + HALSimWSBaseProvider& operator=(const HALSimWSBaseProvider&) = delete; + + // Called when the websocket connects. This will cause providers + // to register their HAL callbacks + virtual void OnNetworkConnected( + std::shared_ptr ws) = 0; + + // Called when the websocket disconnects. This will cause provider + // to cancel their HAL callbacks + virtual void OnNetworkDisconnected() = 0; + + // network -> sim + virtual void OnNetValueChanged(const wpi::json& json); + + const std::string GetDeviceType() { return m_type; } + const std::string GetDeviceId() { return m_deviceId; } + + protected: + // sim -> network + std::weak_ptr m_ws; + std::string m_key; + + std::string m_type; + std::string m_deviceId = ""; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.h b/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.h new file mode 100644 index 0000000000..be3260755b --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.h @@ -0,0 +1,68 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +#include "WSBaseProvider.h" + +namespace wpilibws { + +typedef void (*HALCbRegisterIndexedFunc)(int32_t index, + HAL_NotifyCallback callback, + void* param, HAL_Bool initialNotify); +typedef void (*HALCbRegisterSingleFunc)(HAL_NotifyCallback callback, + void* param, HAL_Bool initialNotify); + +// provider generates diffs based on values +class HALSimWSHalProvider : public HALSimWSBaseProvider { + public: + using HALSimWSBaseProvider::HALSimWSBaseProvider; + + void OnNetworkConnected(std::shared_ptr ws); + void OnNetworkDisconnected(); + + void ProcessHalCallback(const wpi::json& payload); + + protected: + virtual void RegisterCallbacks() = 0; + virtual void CancelCallbacks() = 0; +}; + +// provider generates per-channel diffs +class HALSimWSHalChanProvider : public HALSimWSHalProvider { + public: + explicit HALSimWSHalChanProvider(int32_t channel, const std::string& key, + const std::string& type); + + int32_t GetChannel() { return m_channel; } + + protected: + int32_t m_channel; +}; + +using WSRegisterFunc = std::function)>; + +template +void CreateProviders(const std::string& prefix, int32_t numChannels, + WSRegisterFunc webRegisterFunc); + +template +void CreateSingleProvider(const std::string& key, + WSRegisterFunc webRegisterFunc); + +#include "WSHalProviders.inl" + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.inl b/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.inl new file mode 100644 index 0000000000..6bcdcdd57a --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.inl @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +template +void CreateProviders(const std::string& prefix, int numChannels, + WSRegisterFunc webRegisterFunc) { + for (int32_t i = 0; i < numChannels; i++) { + auto key = (prefix + "/" + wpi::Twine(i)).str(); + auto ptr = std::make_unique(i, key, prefix); + webRegisterFunc(key, std::move(ptr)); + } +} + +template +void CreateSingleProvider(const std::string& key, + WSRegisterFunc webRegisterFunc) { + auto ptr = std::make_unique(key, key); + webRegisterFunc(key, std::move(ptr)); +} diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProviderContainer.h b/simulation/halsim_ws_core/src/main/native/include/WSProviderContainer.h new file mode 100644 index 0000000000..7cd1240775 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSProviderContainer.h @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include + +#include + +#include "WSBaseProvider.h" + +namespace wpilibws { + +class ProviderContainer { + public: + using ProviderPtr = std::shared_ptr; + using IterFn = std::function; + + ProviderContainer() {} + + ProviderContainer(const ProviderContainer&) = delete; + ProviderContainer& operator=(const ProviderContainer&) = delete; + + void Add(wpi::StringRef key, std::shared_ptr provider) { + std::unique_lock lock(m_mutex); + m_providers[key] = provider; + } + + void Delete(wpi::StringRef key) { + std::unique_lock lock(m_mutex); + m_providers.erase(key); + } + + void ForEach(IterFn fn) { + std::shared_lock lock(m_mutex); + for (auto& kv : m_providers) { + fn(kv.getValue()); + } + } + + ProviderPtr Get(wpi::StringRef key) { + std::shared_lock lock(m_mutex); + auto fiter = m_providers.find(key); + return fiter != m_providers.end() ? fiter->second : ProviderPtr(); + } + + private: + std::shared_mutex m_mutex; + wpi::StringMap> m_providers; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_Analog.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Analog.h new file mode 100644 index 0000000000..2c59139043 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Analog.h @@ -0,0 +1,57 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "WSHalProviders.h" + +namespace wpilibws { + +class HALSimWSProviderAnalogIn : public HALSimWSHalChanProvider { + public: + static void Initialize(WSRegisterFunc webRegisterFunc); + + using HALSimWSHalChanProvider::HALSimWSHalChanProvider; + ~HALSimWSProviderAnalogIn(); + + void OnNetValueChanged(const wpi::json& json) override; + + protected: + void RegisterCallbacks() override; + void CancelCallbacks() override; + + private: + int32_t m_initCbKey = 0; + int32_t m_avgbitsCbKey = 0; + int32_t m_oversampleCbKey = 0; + int32_t m_voltageCbKey = 0; + int32_t m_accumInitCbKey = 0; + int32_t m_accumValueCbKey = 0; + int32_t m_accumCountCbKey = 0; + int32_t m_accumCenterCbKey = 0; + int32_t m_accumDeadbandCbKey = 0; +}; + +class HALSimWSProviderAnalogOut : public HALSimWSHalChanProvider { + public: + static void Initialize(WSRegisterFunc webRegisterFunc); + + using HALSimWSHalChanProvider::HALSimWSHalChanProvider; + ~HALSimWSProviderAnalogOut(); + + protected: + void RegisterCallbacks() override; + void CancelCallbacks() override; + + private: + int32_t m_initCbKey = 0; + int32_t m_voltageCbKey = 0; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_DIO.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_DIO.h new file mode 100644 index 0000000000..7ada2f7eee --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_DIO.h @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "WSHalProviders.h" + +namespace wpilibws { + +class HALSimWSProviderDIO : public HALSimWSHalChanProvider { + public: + static void Initialize(WSRegisterFunc webRegisterFunc); + + using HALSimWSHalChanProvider::HALSimWSHalChanProvider; + ~HALSimWSProviderDIO(); + + void OnNetValueChanged(const wpi::json& json) override; + + protected: + void RegisterCallbacks() override; + void CancelCallbacks() override; + + private: + int32_t m_initCbKey = 0; + int32_t m_valueCbKey = 0; + int32_t m_pulseLengthCbKey = 0; + int32_t m_inputCbKey = 0; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_DriverStation.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_DriverStation.h new file mode 100644 index 0000000000..7c8cb2bee3 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_DriverStation.h @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "WSHalProviders.h" + +namespace wpilibws { + +class HALSimWSProviderDriverStation : public HALSimWSHalProvider { + public: + static void Initialize(WSRegisterFunc webRegisterFunc); + + using HALSimWSHalProvider::HALSimWSHalProvider; + ~HALSimWSProviderDriverStation(); + + void OnNetValueChanged(const wpi::json& json) override; + + protected: + void RegisterCallbacks() override; + void CancelCallbacks() override; + + private: + int32_t m_enabledCbKey = 0; + int32_t m_autonomousCbKey = 0; + int32_t m_testCbKey = 0; + int32_t m_estopCbKey = 0; + int32_t m_fmsCbKey = 0; + int32_t m_dsCbKey = 0; + int32_t m_allianceCbKey = 0; + int32_t m_matchTimeCbKey = 0; + int32_t m_newDataCbKey = 0; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h new file mode 100644 index 0000000000..fe2a82dd05 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h @@ -0,0 +1,38 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "WSHalProviders.h" + +namespace wpilibws { + +class HALSimWSProviderEncoder : public HALSimWSHalChanProvider { + public: + static void Initialize(WSRegisterFunc webRegisterFunc); + + using HALSimWSHalChanProvider::HALSimWSHalChanProvider; + ~HALSimWSProviderEncoder(); + + void OnNetValueChanged(const wpi::json& json) override; + + protected: + void RegisterCallbacks() override; + void CancelCallbacks() override; + + private: + int32_t m_initCbKey = 0; + int32_t m_countCbKey = 0; + int32_t m_periodCbKey = 0; + int32_t m_resetCbKey = 0; + int32_t m_reverseDirectionCbKey = 0; + int32_t m_samplesCbKey = 0; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_Joystick.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Joystick.h new file mode 100644 index 0000000000..953e6be6db --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Joystick.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "WSHalProviders.h" + +namespace wpilibws { + +class HALSimWSProviderJoystick : public HALSimWSHalChanProvider { + public: + static void Initialize(WSRegisterFunc webRegisterFunc); + + using HALSimWSHalChanProvider::HALSimWSHalChanProvider; + ~HALSimWSProviderJoystick(); + + void OnNetValueChanged(const wpi::json& json) override; + + protected: + void RegisterCallbacks() override; + void CancelCallbacks() override; + + private: + int32_t m_dsNewDataCbKey = 0; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_PWM.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_PWM.h new file mode 100644 index 0000000000..9bce76f77a --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_PWM.h @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "WSHalProviders.h" + +namespace wpilibws { + +class HALSimWSProviderPWM : public HALSimWSHalChanProvider { + public: + static void Initialize(WSRegisterFunc webRegisterFunc); + + using HALSimWSHalChanProvider::HALSimWSHalChanProvider; + ~HALSimWSProviderPWM(); + + protected: + void RegisterCallbacks() override; + void CancelCallbacks() override; + + private: + int32_t m_initCbKey = 0; + int32_t m_speedCbKey = 0; + int32_t m_positionCbKey = 0; + int32_t m_rawCbKey = 0; + int32_t m_periodScaleCbKey = 0; + int32_t m_zeroLatchCbKey = 0; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_Relay.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Relay.h new file mode 100644 index 0000000000..0a7512a730 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_Relay.h @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "WSHalProviders.h" + +namespace wpilibws { + +class HALSimWSProviderRelay : public HALSimWSHalChanProvider { + public: + static void Initialize(WSRegisterFunc webRegisterFunc); + + using HALSimWSHalChanProvider::HALSimWSHalChanProvider; + ~HALSimWSProviderRelay(); + + protected: + void RegisterCallbacks() override; + void CancelCallbacks() override; + + private: + int32_t m_initFwdCbKey = 0; + int32_t m_initRevCbKey = 0; + int32_t m_fwdCbKey = 0; + int32_t m_revCbKey = 0; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_RoboRIO.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_RoboRIO.h new file mode 100644 index 0000000000..fcd7a2e0f1 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_RoboRIO.h @@ -0,0 +1,47 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "WSHalProviders.h" + +namespace wpilibws { + +class HALSimWSProviderRoboRIO : public HALSimWSHalProvider { + public: + static void Initialize(WSRegisterFunc webRegisterFunc); + + using HALSimWSHalProvider::HALSimWSHalProvider; + ~HALSimWSProviderRoboRIO(); + + void OnNetValueChanged(const wpi::json& json) override; + + protected: + void RegisterCallbacks() override; + void CancelCallbacks() override; + + private: + int32_t m_fpgaCbKey = 0; + int32_t m_vinVoltageCbKey = 0; + int32_t m_vinCurrentCbKey = 0; + int32_t m_6vVoltageCbKey = 0; + int32_t m_6vCurrentCbKey = 0; + int32_t m_6vActiveCbKey = 0; + int32_t m_6vFaultsCbKey = 0; + int32_t m_5vVoltageCbKey = 0; + int32_t m_5vCurrentCbKey = 0; + int32_t m_5vActiveCbKey = 0; + int32_t m_5vFaultsCbKey = 0; + int32_t m_3v3VoltageCbKey = 0; + int32_t m_3v3CurrentCbKey = 0; + int32_t m_3v3ActiveCbKey = 0; + int32_t m_3v3FaultsCbKey = 0; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_SimDevice.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_SimDevice.h new file mode 100644 index 0000000000..cde27409f7 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_SimDevice.h @@ -0,0 +1,128 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include "WSBaseProvider.h" +#include "WSProviderContainer.h" + +namespace wpilibws { + +class HALSimWSProviderSimDevice; +class HALSimWSProviderSimDevices; + +struct SimDeviceValueData { + HALSimWSProviderSimDevice* device; + HAL_SimValueHandle handle; + std::string key; + HAL_Type valueType; +}; + +class HALSimWSProviderSimDevice : public HALSimWSBaseProvider { + public: + HALSimWSProviderSimDevice(HAL_SimDeviceHandle handle, const std::string& key, + const std::string& deviceId) + : HALSimWSBaseProvider(key, "SimDevices"), m_handle(handle) { + m_deviceId = deviceId; + } + + ~HALSimWSProviderSimDevice(); + + void OnNetworkConnected( + std::shared_ptr ws) override; + + void OnNetworkDisconnected() override; + + void OnNetValueChanged(const wpi::json& json) override; + + void ProcessHalCallback(const wpi::json& payload); + + private: + static void OnValueCreatedStatic(const char* name, void* param, + HAL_SimValueHandle handle, HAL_Bool readonly, + const struct HAL_Value* value) { + (reinterpret_cast(param)) + ->OnValueCreated(name, handle, readonly, value); + } + void OnValueCreated(const char* name, HAL_SimValueHandle handle, + HAL_Bool readonly, const struct HAL_Value* value); + + static void OnValueChangedStatic(const char* name, void* param, + HAL_SimValueHandle handle, HAL_Bool readonly, + const struct HAL_Value* value) { + auto valueData = (reinterpret_cast(param)); + valueData->device->OnValueChanged(valueData, value); + } + void OnValueChanged(SimDeviceValueData* valueData, + const struct HAL_Value* value); + + void CancelCallbacks(); + + wpi::StringMap> m_valueHandles; + std::shared_mutex m_vhLock; + + HAL_SimDeviceHandle m_handle; + + std::shared_ptr m_simDevicesBase; + + int32_t m_simValueCreatedCbKey = 0; + wpi::StringMap m_simValueChangedCbKeys; +}; + +class HALSimWSProviderSimDevices { + public: + using LoopFn = std::function; + using UvExecFn = wpi::uv::AsyncFunction; + + explicit HALSimWSProviderSimDevices(ProviderContainer& providers) + : m_providers(providers) {} + ~HALSimWSProviderSimDevices(); + + void Initialize(std::shared_ptr loop); + + void OnNetworkConnected(std::shared_ptr hws); + void OnNetworkDisconnected(); + + std::shared_ptr GetWSConnection() { + return m_ws; + } + + private: + static void DeviceCreatedCallbackStatic(const char* name, void* param, + HAL_SimDeviceHandle handle) { + (reinterpret_cast(param)) + ->DeviceCreatedCallback(name, handle); + } + void DeviceCreatedCallback(const char* name, HAL_SimDeviceHandle handle); + + static void DeviceFreedCallbackStatic(const char* name, void* param, + HAL_SimDeviceHandle handle) { + (reinterpret_cast(param)) + ->DeviceFreedCallback(name, handle); + } + void DeviceFreedCallback(const char* name, HAL_SimDeviceHandle handle); + + void CancelCallbacks(); + + ProviderContainer& m_providers; + + std::shared_ptr m_ws; + std::shared_ptr m_exec; + + int32_t m_deviceCreatedCbKey = 0; + int32_t m_deviceFreedCbKey = 0; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_core/src/main/native/include/WSProvider_dPWM.h b/simulation/halsim_ws_core/src/main/native/include/WSProvider_dPWM.h new file mode 100644 index 0000000000..78dc96c944 --- /dev/null +++ b/simulation/halsim_ws_core/src/main/native/include/WSProvider_dPWM.h @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "WSHalProviders.h" + +namespace wpilibws { + +class HALSimWSProviderDigitalPWM : public HALSimWSHalChanProvider { + public: + static void Initialize(WSRegisterFunc webRegisterFunc); + + using HALSimWSHalChanProvider::HALSimWSHalChanProvider; + ~HALSimWSProviderDigitalPWM(); + + protected: + void RegisterCallbacks() override; + void CancelCallbacks() override; + + private: + int32_t m_initCbKey = 0; + int32_t m_dutyCycleCbKey = 0; + int32_t m_pinCbKey = 0; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_server/CMakeLists.txt b/simulation/halsim_ws_server/CMakeLists.txt new file mode 100644 index 0000000000..d1c7983e36 --- /dev/null +++ b/simulation/halsim_ws_server/CMakeLists.txt @@ -0,0 +1,16 @@ +project(halsim_ws_server) + +include(CompileWarnings) + +file(GLOB halsim_ws_server_src src/main/native/cpp/*.cpp) + +add_library(halsim_ws_server MODULE ${halsim_ws_server_src}) +wpilib_target_warnings(halsim_ws_server) +set_target_properties(halsim_ws_server PROPERTIES DEBUG_POSTFIX "d") +target_link_libraries(halsim_ws_server PUBLIC hal halsim_ws_core) + +target_include_directories(halsim_ws_server PRIVATE src/main/native/include) + +set_property(TARGET halsim_ws_server PROPERTY FOLDER "libraries") + +install(TARGETS halsim_ws_server EXPORT halsim_ws_server DESTINATION "${main_lib_dest}") diff --git a/simulation/halsim_ws_server/build.gradle b/simulation/halsim_ws_server/build.gradle new file mode 100644 index 0000000000..6c6be48f7a --- /dev/null +++ b/simulation/halsim_ws_server/build.gradle @@ -0,0 +1,72 @@ + +description = "WebSocket Server Extension" + +ext { + includeWpiutil = true + pluginName = 'halsim_ws_server' +} + +apply plugin: 'google-test-test-suite' + + +ext { + staticGtestConfigs = [:] +} + +staticGtestConfigs["${pluginName}Test"] = [] +apply from: "${rootDir}/shared/googletest.gradle" + +apply from: "${rootDir}/shared/plugins/setupBuild.gradle" + +model { + testSuites { + def comps = $.components + if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxraspbian') && !project.hasProperty('onlylinuxaarch64bionic')) { + "${pluginName}Test"(GoogleTestTestSuiteSpec) { + for(NativeComponentSpec c : comps) { + if (c.name == pluginName) { + testing c + break + } + } + sources { + cpp { + source { + srcDirs 'src/test/native/cpp' + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/test/native/include', 'src/main/native/cpp' + } + } + } + } + } + } + + binaries { + all { + if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { + it.buildable = false + return + } + + lib project: ":simulation:halsim_ws_core", library: "halsim_ws_core", linkage: "static" + } + + withType(GoogleTestTestSuiteBinarySpec) { + project(':hal').addHalDependency(it, 'shared') + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' + lib library: pluginName, linkage: 'shared' + if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) { + nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared') + } + } + } + +} + +tasks.withType(RunTestExecutable) { + args "--gtest_output=xml:test_detail.xml" + outputs.dir outputDir +} diff --git a/simulation/halsim_ws_server/src/dev/native/cpp/main.cpp b/simulation/halsim_ws_server/src/dev/native/cpp/main.cpp new file mode 100644 index 0000000000..c98c5e4e28 --- /dev/null +++ b/simulation/halsim_ws_server/src/dev/native/cpp/main.cpp @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include + +#include +#include +#include +#include +#include + +extern "C" int HALSIM_InitExtension(void); + +int main() { + HAL_Initialize(500, 0); + HALSIM_InitExtension(); + + // HAL_ObserveUserProgramStarting(); + + HAL_RunMain(); + + int cycleCount = 0; + while (cycleCount < 1000) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + cycleCount++; + std::cout << "Count: " << cycleCount << std::endl; + } + + std::cout << "DONE" << std::endl; + HAL_ExitMain(); +} diff --git a/simulation/halsim_ws_server/src/main/native/cpp/HALSimHttpConnection.cpp b/simulation/halsim_ws_server/src/main/native/cpp/HALSimHttpConnection.cpp new file mode 100644 index 0000000000..2a60bf2e48 --- /dev/null +++ b/simulation/halsim_ws_server/src/main/native/cpp/HALSimHttpConnection.cpp @@ -0,0 +1,299 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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 "HALSimHttpConnection.h" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "HALSimWSServer.h" + +namespace uv = wpi::uv; + +namespace wpilibws { + +bool HALSimHttpConnection::IsValidWsUpgrade(wpi::StringRef protocol) { + auto hws = HALSimWeb::GetInstance(); + if (m_request.GetUrl() != hws->GetServerUri()) { + MySendError(404, "invalid websocket address"); + return false; + } + + return true; +} + +void HALSimHttpConnection::ProcessWsUpgrade() { + m_websocket->open.connect_extended([this](auto conn, wpi::StringRef) { + conn.disconnect(); // one-shot + + m_buffers = std::make_unique(); + m_exec = + UvExecFunc::Create(m_stream.GetLoop(), [](auto out, LoopFunc func) { + func(); + out.set_value(); + }); + + auto hws = HALSimWeb::GetInstance(); + if (!hws) { + Log(503); + m_websocket->Fail(503, "HALSimWeb unavailable"); + return; + } + + if (!hws->RegisterWebsocket(shared_from_this())) { + Log(409); + m_websocket->Fail(409, "Only a single simulation websocket is allowed"); + return; + } + + Log(200); + m_isWsConnected = true; + wpi::errs() << "HALWebSim: websocket connected\n"; + }); + + // parse incoming JSON, dispatch to parent + m_websocket->text.connect([this](wpi::StringRef msg, bool) { + auto hws = HALSimWeb::GetInstance(); + if (!m_isWsConnected || !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(); + m_websocket->Fail(400, err); + return; + } + hws->OnNetValueChanged(j); + }); + + m_websocket->closed.connect([this](uint16_t, wpi::StringRef) { + // unset the global, allow another websocket to connect + if (m_isWsConnected) { + wpi::errs() << "HALWebSim: websocket disconnected\n"; + m_isWsConnected = false; + + auto hws = HALSimWeb::GetInstance(); + if (hws) { + hws->CloseWebsocket(shared_from_this()); + } + } + }); +} + +void HALSimHttpConnection::OnSimValueChanged(const wpi::json& msg) { + // render json to buffers + wpi::SmallVector 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(); + } + }); + }); +} + +class SendfileReq : public uv::RequestImpl { + public: + SendfileReq(uv_file out, uv_file in, int64_t inOffset, size_t len) + : m_out(out), m_in(in), m_inOffset(inOffset), m_len(len) { + error = [this](uv::Error err) { GetLoop().error(err); }; + } + + uv::Loop& GetLoop() const { + return *static_cast(GetRaw()->loop->data); + } + + int Send(uv::Loop& loop) { + int err = uv_fs_sendfile(loop.GetRaw(), GetRaw(), m_out, m_in, m_inOffset, + m_len, [](uv_fs_t* req) { + auto& h = *static_cast(req->data); + if (req->result < 0) { + h.ReportError(req->result); + h.complete(); + h.Release(); + return; + } + + h.m_inOffset += req->result; + h.m_len -= req->result; + if (h.m_len == 0) { + // done + h.complete(); + h.Release(); // this is always a one-shot + return; + } + + // need to send more + h.Send(h.GetLoop()); + }); + if (err < 0) { + ReportError(err); + complete(); + } + return err; + } + + wpi::sig::Signal<> complete; + + private: + uv_file m_out; + uv_file m_in; + int64_t m_inOffset; + size_t m_len; +}; + +void Sendfile(uv::Loop& loop, uv_file out, uv_file in, int64_t inOffset, + size_t len, std::function complete) { + auto req = std::make_shared(out, in, inOffset, len); + if (complete) req->complete.connect(complete); + int err = req->Send(loop); + if (err >= 0) req->Keep(); +} + +void HALSimHttpConnection::SendFileResponse(int code, + const wpi::Twine& codeText, + const wpi::Twine& contentType, + const wpi::Twine& filename, + const wpi::Twine& extraHeader) { + // open file + int infd; + if (wpi::sys::fs::openFileForRead(filename, infd)) { + MySendError(404, "error opening file"); + return; + } + + // get status (to get file size) + wpi::sys::fs::file_status status; + if (wpi::sys::fs::status(infd, status)) { + MySendError(404, "error getting file size"); + wpi::sys::fs::file_t file = uv_get_osfhandle(infd); + wpi::sys::fs::closeFile(file); + return; + } + + uv_os_fd_t outfd; + int err = uv_fileno(m_stream.GetRawHandle(), &outfd); + if (err < 0) { + m_stream.GetLoopRef().ReportError(err); + MySendError(404, "error getting fd"); + wpi::sys::fs::file_t file = uv_get_osfhandle(infd); + wpi::sys::fs::closeFile(file); + return; + } + + wpi::SmallVector toSend; + wpi::raw_uv_ostream os{toSend, 4096}; + BuildHeader(os, code, codeText, contentType, status.getSize(), extraHeader); + SendData(os.bufs(), false); + + Log(code); + + // Read the file byte by byte + wpi::SmallVector bodyData; + wpi::raw_uv_ostream bodyOs{bodyData, 4096}; + + wpi::raw_fd_istream is{infd, true}; + std::string fileBuf; + size_t oldSize = 0; + + while (fileBuf.size() < status.getSize()) { + oldSize = fileBuf.size(); + fileBuf.resize(oldSize + 1); + is.read(&(*fileBuf.begin()) + oldSize, 1); + } + + bodyOs << fileBuf; + + SendData(bodyOs.bufs(), false); + if (!m_keepAlive) { + m_stream.Close(); + } +} + +void HALSimHttpConnection::ProcessRequest() { + wpi::UrlParser url{m_request.GetUrl(), + m_request.GetMethod() == wpi::HTTP_CONNECT}; + if (!url.IsValid()) { + // failed to parse URL + MySendError(400, "Invalid URL"); + return; + } + + wpi::StringRef path; + if (url.HasPath()) path = url.GetPath(); + + if (m_request.GetMethod() == wpi::HTTP_GET && path.startswith("/") && + !path.contains("..")) { + // convert to fs native representation + wpi::SmallVector nativePath; + wpi::sys::path::native(path, nativePath); + + if (path.startswith("/user/")) { + std::string prefix = (wpi::sys::path::get_separator() + "user" + + wpi::sys::path::get_separator()) + .str(); + wpi::sys::path::replace_path_prefix(nativePath, prefix, m_webroot_user); + } else { + wpi::sys::path::replace_path_prefix( + nativePath, wpi::sys::path::get_separator(), m_webroot_sys); + } + + if (wpi::sys::fs::is_directory(nativePath)) { + wpi::sys::path::append(nativePath, "index.html"); + } + + if (!wpi::sys::fs::exists(nativePath) || + wpi::sys::fs::is_directory(nativePath)) { + MySendError(404, "Resource '" + path + "' not found"); + } else { + auto contentType = wpi::MimeTypeFromPath(wpi::Twine(nativePath).str()); + SendFileResponse(200, "OK", contentType, nativePath); + } + } else { + MySendError(404, "Resource not found"); + } +} + +void HALSimHttpConnection::MySendError(int code, const wpi::Twine& message) { + Log(code); + SendError(code, message); +} + +void HALSimHttpConnection::Log(int code) { + auto method = wpi::http_method_str(m_request.GetMethod()); + wpi::errs() << method << " " << m_request.GetUrl() << " HTTP/" + << m_request.GetMajor() << "." << m_request.GetMinor() << " " + << code << "\n"; +} + +} // namespace wpilibws diff --git a/simulation/halsim_ws_server/src/main/native/cpp/HALSimWSServer.cpp b/simulation/halsim_ws_server/src/main/native/cpp/HALSimWSServer.cpp new file mode 100644 index 0000000000..6bebe33f3a --- /dev/null +++ b/simulation/halsim_ws_server/src/main/native/cpp/HALSimWSServer.cpp @@ -0,0 +1,181 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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 "HALSimWSServer.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "HALSimHttpConnection.h" + +namespace uv = wpi::uv; + +namespace wpilibws { + +std::shared_ptr HALSimWeb::g_instance; + +bool HALSimWeb::Initialize() { + // determine where to get static content from + // wpi::SmallVector tmp; + wpi::SmallString<64> tmp; + + const char* webroot_sys = std::getenv("HALSIMWS_SYSROOT"); + if (webroot_sys != NULL) { + wpi::StringRef tstr(webroot_sys); + tmp.append(tstr); + } else { + wpi::sys::fs::current_path(tmp); + wpi::sys::path::append(tmp, "sim"); + } + wpi::sys::fs::make_absolute(tmp); + m_webroot_sys = wpi::Twine(tmp).str(); + + tmp.clear(); + const char* webroot_user = std::getenv("HALSIMWS_USERROOT"); + if (webroot_user != NULL) { + wpi::StringRef tstr(webroot_user); + tmp.append(tstr); + } else { + wpi::sys::fs::current_path(tmp); + wpi::sys::path::append(tmp, "sim", "user"); + } + wpi::sys::fs::make_absolute(tmp); + m_webroot_user = wpi::Twine(tmp).str(); + + const char* uri = std::getenv("HALSIMWS_URI"); + if (uri != NULL) { + m_uri = uri; + } else { + m_uri = "/wpilibws"; + } + + 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; + } + + // create libuv things + m_loop = uv::Loop::Create(); + if (!m_loop) { + return false; + } + + m_loop->error.connect([](uv::Error err) { + wpi::errs() << "HALSim WS Server libuv ERROR: " << err.str() << '\n'; + }); + + m_server = uv::Tcp::Create(m_loop); + if (!m_server) { + return false; + } + + m_server->Bind("", m_port); + return true; +} + +void HALSimWeb::Main(void* param) { + GetInstance()->MainLoop(); + SetInstance(nullptr); +} + +void HALSimWeb::MainLoop() { + // when we get a connection, accept it and start reading + m_server->connection.connect([this, srv = m_server.get()] { + auto tcp = srv->Accept(); + if (!tcp) return; + + tcp->SetNoDelay(true); + + auto conn = std::make_shared(tcp, m_webroot_sys, + m_webroot_user); + tcp->SetData(conn); + }); + + // start listening for incoming connections + m_server->Listen(); + wpi::outs() << "Listening at http://localhost:" << m_port << "\n"; + wpi::outs() << "WebSocket URI: " << m_uri << "\n"; + m_loop->Run(); +} + +void HALSimWeb::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 HALSimWeb::RegisterWebsocket( + std::shared_ptr hws) { + if (m_hws.lock()) { + return false; + } + + m_hws = hws; + + m_simDevicesProvider.OnNetworkConnected(hws); + + // notify all providers that they should use this new websocket instead + m_providers.ForEach([hws](std::shared_ptr provider) { + provider->OnNetworkConnected(hws); + }); + + return true; +} + +void HALSimWeb::CloseWebsocket( + std::shared_ptr hws) { + // Inform the providers that they need to cancel callbacks + m_simDevicesProvider.OnNetworkDisconnected(); + + m_providers.ForEach([](std::shared_ptr provider) { + provider->OnNetworkDisconnected(); + }); + + if (hws == m_hws.lock()) { + m_hws.reset(); + } +} + +void HALSimWeb::OnNetValueChanged(const wpi::json& msg) { + // Look for "type" and "device" fields so that we can + // generate the key + + try { + std::string type = msg.at("type").get(); + std::string device = msg.at("device").get(); + auto data = msg.at("data"); + + auto key = type + "/" + device; + auto provider = m_providers.Get(key); + if (provider) { + provider->OnNetValueChanged(data); + } + } catch (wpi::json::exception& e) { + wpi::errs() << "Error with incoming message: " << e.what() << "\n"; + } +} + +} // namespace wpilibws diff --git a/simulation/halsim_ws_server/src/main/native/cpp/main.cpp b/simulation/halsim_ws_server/src/main/native/cpp/main.cpp new file mode 100644 index 0000000000..498031945e --- /dev/null +++ b/simulation/halsim_ws_server/src/main/native/cpp/main.cpp @@ -0,0 +1,67 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "HALSimWSServer.h" + +using namespace std::placeholders; +using namespace wpilibws; + +// Currently, robots never terminate, so we keep static objects that are +// never properly released or cleaned up. +static ProviderContainer providers; +static HALSimWSProviderSimDevices simDevices(providers); + +extern "C" { +#if defined(WIN32) || defined(_WIN32) +__declspec(dllexport) +#endif + int HALSIM_InitExtension(void) { + wpi::outs() << "Websocket Simulator Initializing.\n"; + auto hsw = std::make_shared(providers, simDevices); + HALSimWeb::SetInstance(hsw); + + if (!hsw->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(hsw->GetLoop()); + + HAL_SetMain(nullptr, HALSimWeb::Main, HALSimWeb::Exit); + + wpi::outs() << "Websocket Simulator Initialized!\n"; + return 0; +} +} // extern "C" diff --git a/simulation/halsim_ws_server/src/main/native/include/HALSimHttpConnection.h b/simulation/halsim_ws_server/src/main/native/include/HALSimHttpConnection.h new file mode 100644 index 0000000000..5b9ab49903 --- /dev/null +++ b/simulation/halsim_ws_server/src/main/native/include/HALSimHttpConnection.h @@ -0,0 +1,74 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace wpilibws { + +class HALSimWeb; + +class HALSimHttpConnection + : public wpi::HttpWebSocketServerConnection, + public HALSimBaseWebSocketConnection { + public: + using BufferPool = wpi::uv::SimpleBufferPool<4>; + + using LoopFunc = std::function; + using UvExecFunc = wpi::uv::AsyncFunction; + + explicit HALSimHttpConnection(std::shared_ptr stream, + wpi::StringRef webroot_sys, + wpi::StringRef webroot_user) + : wpi::HttpWebSocketServerConnection(stream, {}), + m_webroot_sys(webroot_sys), + m_webroot_user(webroot_user) {} + + public: + // callable from any thread + void OnSimValueChanged(const wpi::json& msg) override; + + protected: + void ProcessRequest() override; + bool IsValidWsUpgrade(wpi::StringRef protocol) override; + void ProcessWsUpgrade() override; + void SendFileResponse(int code, const wpi::Twine& codeText, + const wpi::Twine& contentType, + const wpi::Twine& filename, + const wpi::Twine& extraHeader = wpi::Twine{}); + + void MySendError(int code, const wpi::Twine& message); + void Log(int code); + + private: + // Absolute paths of folders to retrieve data from + // -> / + std::string m_webroot_sys; + // -> /user + std::string m_webroot_user; + + // is the websocket connected? + bool m_isWsConnected = false; + + // these are only valid if the websocket is connected + std::shared_ptr m_exec; + std::unique_ptr m_buffers; + std::mutex m_buffers_mutex; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_server/src/main/native/include/HALSimWSServer.h b/simulation/halsim_ws_server/src/main/native/include/HALSimWSServer.h new file mode 100644 index 0000000000..22c5a31c0c --- /dev/null +++ b/simulation/halsim_ws_server/src/main/native/include/HALSimWSServer.h @@ -0,0 +1,81 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace wpilibws { + +class HALSimHttpConnection; + +class HALSimWeb { + public: + static std::shared_ptr GetInstance() { return g_instance; } + static void SetInstance(std::shared_ptr inst) { + g_instance = inst; + } + + explicit HALSimWeb(ProviderContainer& providers, + HALSimWSProviderSimDevices& simDevicesProvider) + : m_providers(providers), m_simDevicesProvider(simDevicesProvider) {} + + HALSimWeb(const HALSimWeb&) = delete; + HALSimWeb& operator=(const HALSimWeb&) = delete; + + bool Initialize(); + static void Main(void*); + static void Exit(void*); + + bool RegisterWebsocket(std::shared_ptr hws); + void CloseWebsocket(std::shared_ptr hws); + + // network -> sim + void OnNetValueChanged(const wpi::json& msg); + + std::string GetServerUri() const { return m_uri; } + int GetServerPort() { return m_port; } + std::shared_ptr GetLoop() { return m_loop; } + + private: + static std::shared_ptr g_instance; + + void MainLoop(); + + // connected http connection that contains active websocket + std::weak_ptr m_hws; + + // list of providers + ProviderContainer& m_providers; + HALSimWSProviderSimDevices& m_simDevicesProvider; + + std::shared_ptr m_loop; + std::shared_ptr m_server; + + // Absolute paths of folders to retrieve data from + // -> / + std::string m_webroot_sys; + // -> /user + std::string m_webroot_user; + + std::string m_uri; + int m_port; +}; + +} // namespace wpilibws diff --git a/simulation/halsim_ws_server/src/test/native/cpp/WebServerClientTest.cpp b/simulation/halsim_ws_server/src/test/native/cpp/WebServerClientTest.cpp new file mode 100644 index 0000000000..cf0d7e94bc --- /dev/null +++ b/simulation/halsim_ws_server/src/test/native/cpp/WebServerClientTest.cpp @@ -0,0 +1,177 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "WebServerClientTest.h" + +#include + +#include +#include +#include +#include + +static constexpr int kTcpConnectAttemptTimeout = 1000; + +namespace uv = wpi::uv; + +namespace wpilibws { + +std::shared_ptr WebServerClientTest::g_instance; + +// Create Web Socket and specify event callbacks +void WebServerClientTest::InitializeWebSocket(const std::string& host, int port, + const std::string& uri) { + std::stringstream ss; + ss << host << ":" << port; + wpi::outs() << "Will attempt to connect to: " << ss.str() << uri << "\n"; + m_websocket = + wpi::WebSocket::CreateClient(*m_tcp_client.get(), uri, ss.str()); + + // Hook up events + m_websocket->open.connect_extended([this](auto conn, wpi::StringRef) { + conn.disconnect(); + m_buffers = std::make_unique(); + + m_exec = UvExecFunc::Create(m_tcp_client->GetLoop(), + [](auto out, LoopFunc func) { + func(); + out.set_value(); + }); + + m_ws_connected = true; + wpi::errs() << "WebServerClientTest: WebSocket Connected\n"; + }); + + m_websocket->text.connect([this](wpi::StringRef msg, bool) { + 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; + } + // Save last message received + m_json = j; + + // If terminate flag set, end loop after message recieved + if (m_terminateFlag) { + m_loop->Stop(); + } + }); + + m_websocket->closed.connect([this](uint16_t, wpi::StringRef) { + if (m_ws_connected) { + wpi::errs() << "WebServerClientTest: Websocket Disconnected\n"; + m_ws_connected = false; + } + }); +} + +// Create tcp client, specify callbacks, and create timers for loop +bool WebServerClientTest::Initialize(std::shared_ptr& loop) { + m_loop = loop; + m_loop->error.connect( + [](uv::Error err) { wpi::errs() << "uv Error: " << err.str() << "\n"; }); + + m_tcp_client = uv::Tcp::Create(m_loop); + if (!m_tcp_client) { + wpi::errs() << "ERROR: Could not create TCP Client\n"; + return false; + } + + // 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::errs() << "TCP connection closed\n"; }); + + // Set up the connection timer + m_connect_timer = uv::Timer::Create(m_loop); + if (!m_connect_timer) { + return false; + } + // Set up the timer to attempt connection + m_connect_timer->timeout.connect([this] { AttemptConnect(); }); + m_connect_timer->Start(uv::Timer::Time(0)); + + wpi::outs() << "WebServerClientTest Initialized\n"; + + return true; +} + +void WebServerClientTest::AttemptConnect() { + m_connect_attempts++; + wpi::outs() << "Test Client Connection Attempt " << m_connect_attempts + << "\n"; + + if (m_connect_attempts >= 5) { + wpi::errs() << "Test Client Timeout. Unable to connect\n"; + Exit(); + return; + } + + struct sockaddr_in dest; + uv::NameToAddr("localhost", 8080, &dest); + m_tcp_client->Connect(dest, [this]() { + m_tcp_connected = true; + InitializeWebSocket("localhost", 8080, "/wpilibws"); + }); +} + +void WebServerClientTest::Exit() { + m_loop->Walk([](uv::Handle& h) { + h.SetLoopClosing(true); + h.Close(); + }); +} + +void WebServerClientTest::SendMessage(const wpi::json& msg) { + if (msg.empty()) { + wpi::errs() << "Message to send is empty\n"; + return; + } + + wpi::SmallVector 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(); + } + }); + }); +} + +const wpi::json& WebServerClientTest::GetLastMessage() { return m_json; } + +} // namespace wpilibws diff --git a/simulation/halsim_ws_server/src/test/native/cpp/main.cpp b/simulation/halsim_ws_server/src/test/native/cpp/main.cpp new file mode 100644 index 0000000000..7b6a201558 --- /dev/null +++ b/simulation/halsim_ws_server/src/test/native/cpp/main.cpp @@ -0,0 +1,155 @@ +/*----------------------------------------------------------------------------*/ +/* 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 +#include +#include +#include +#include + +#include "HALSimWSServer.h" +#include "WebServerClientTest.h" +#include "gtest/gtest.h" + +using namespace wpilibws; + +extern "C" int HALSIM_InitExtension( + void); // from simulation/halsim_ws_server/src/main/native/cpp/main.cpp + +const int POLLING_SPEED = 10; // 10 ms polling + +class WebServerIntegrationTest : public ::testing::Test { + public: + WebServerIntegrationTest() { + const int MAX_TEST_TIME = 1000; // 1 second + + // Initialize HAL layer including webserver + HALSIM_InitExtension(); + + // Create and initialize client + m_webserver_client = + std::shared_ptr((new WebServerClientTest())); + WebServerClientTest::SetInstance(m_webserver_client); + auto webserver = HALSimWeb::GetInstance(); + auto loop = webserver->GetLoop(); + m_webserver_client->Initialize(loop); + + // Create failover timer to prevent running forever + m_failoverTimer = wpi::uv::Timer::Create(loop); + m_failoverTimer->timeout.connect([loop] { loop->Stop(); }); + m_failoverTimer->Start(uv::Timer::Time(MAX_TEST_TIME)); + } + + ~WebServerIntegrationTest() { + // Exit HAL layer which exits webserver + HAL_ExitMain(); + HALSimWeb::Exit(nullptr); + + // Exit client + m_webserver_client->Exit(); + WebServerClientTest::SetInstance(nullptr); + + // Unreference timers from loop + m_failoverTimer->Unreference(); + } + + bool IsConnectedClientWS() { return m_webserver_client->IsConnectedWS(); } + void TerminateOnNextMessage(bool flag) { + m_webserver_client->SetTerminateFlag(flag); + } + + private: + std::shared_ptr m_webserver_client; + std::shared_ptr m_failoverTimer; +}; + +TEST_F(WebServerIntegrationTest, DigitalOutput) { + // Create expected results + const bool EXPECTED_VALUE = false; + const int PIN = 0; + + // Attach timer to loop for test function + auto ws = HALSimWeb::GetInstance(); + auto loop = ws->GetLoop(); + auto timer = wpi::uv::Timer::Create(loop); + timer->timeout.connect([&] { + if (IsConnectedClientWS()) { + wpi::outs() << "***** Setting DIO value for pin " << PIN << " to " + << (EXPECTED_VALUE ? "true" : "false") << "\n"; + HALSIM_SetDIOValue(PIN, EXPECTED_VALUE); + TerminateOnNextMessage(true); + } else { + // Recheck in POLLING_SPEED ms + timer->Start(uv::Timer::Time(POLLING_SPEED)); + } + }); + timer->Start(uv::Timer::Time(POLLING_SPEED)); + + HAL_RunMain(); + timer->Unreference(); + + // Get values from JSON message + std::string test_type; + std::string test_device; + bool test_value = true; // Default value from HAL initialization + try { + auto& msg = WebServerClientTest::GetInstance()->GetLastMessage(); + test_type = msg.at("type").get_ref(); + test_device = msg.at("device").get_ref(); + auto& data = msg.at("data"); + wpi::json::const_iterator it = data.find("<>value"); + if (it != data.end()) { + test_value = it.value(); + } + } catch (wpi::json::exception& e) { + wpi::errs() << "Error with incoming message: " << e.what() << "\n"; + } + + // Compare results + EXPECT_EQ("DIO", test_type); + EXPECT_EQ(std::to_string(PIN), test_device); + EXPECT_EQ(EXPECTED_VALUE, test_value); +} + +TEST_F(WebServerIntegrationTest, DigitalInput) { + // Create expected results + const bool EXPECTED_VALUE = false; + const int PIN = 0; + + // Attach timer to loop for test function + auto ws = HALSimWeb::GetInstance(); + auto loop = ws->GetLoop(); + auto timer = wpi::uv::Timer::Create(loop); + timer->timeout.connect([&] { + if (IsConnectedClientWS()) { + wpi::json msg = {{"type", "DIO"}, + {"device", std::to_string(PIN)}, + {"data", {{"<>value", EXPECTED_VALUE}}}}; + wpi::outs() << "***** Input JSON: " << msg.dump() << "\n"; + WebServerClientTest::GetInstance()->SendMessage(msg); + loop->Stop(); + } else { + // Recheck in POLLING_SPEED ms + timer->Start(uv::Timer::Time(POLLING_SPEED)); + } + }); + timer->Start(uv::Timer::Time(POLLING_SPEED)); + + HAL_RunMain(); + timer->Unreference(); + + // Compare results + bool test_value = HALSIM_GetDIOValue(PIN); + EXPECT_EQ(EXPECTED_VALUE, test_value); +} + +int main(int argc, char* argv[]) { + ::testing::InitGoogleTest(&argc, argv); + HAL_Initialize(500, 0); + int ret = RUN_ALL_TESTS(); + return ret; +} diff --git a/simulation/halsim_ws_server/src/test/native/include/WebServerClientTest.h b/simulation/halsim_ws_server/src/test/native/include/WebServerClientTest.h new file mode 100644 index 0000000000..da2eec81bf --- /dev/null +++ b/simulation/halsim_ws_server/src/test/native/include/WebServerClientTest.h @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* 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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uv = wpi::uv; + +namespace wpilibws { + +class WebServerClientTest { + public: + using BufferPool = wpi::uv::SimpleBufferPool<4>; + using LoopFunc = std::function; + using UvExecFunc = wpi::uv::AsyncFunction; + + static std::shared_ptr GetInstance() { + return g_instance; + } + static void SetInstance(std::shared_ptr inst) { + g_instance = inst; + } + + WebServerClientTest() {} + WebServerClientTest(const WebServerClientTest&) = delete; + WebServerClientTest& operator=(const WebServerClientTest&) = delete; + + bool Initialize(std::shared_ptr& loop); + void AttemptConnect(); + void Exit(); + + void SendMessage(const wpi::json& msg); + const wpi::json& GetLastMessage(); + bool IsConnectedWS() { return m_ws_connected; } + void SetTerminateFlag(bool flag) { m_terminateFlag = flag; } + + private: + void InitializeWebSocket(const std::string& host, int port, + const std::string& uri); + + bool m_terminateFlag = false; + static std::shared_ptr g_instance; + bool m_tcp_connected = false; + std::shared_ptr m_connect_timer; + int m_connect_attempts = 0; + std::shared_ptr m_loop; + std::shared_ptr m_tcp_client; + wpi::json m_json; + + bool m_ws_connected = false; + std::shared_ptr m_websocket; + std::shared_ptr m_exec; + std::unique_ptr m_buffers; + std::mutex m_buffers_mutex; +}; + +} // namespace wpilibws