[sim] Add XRP-specific plugin (#5631)

Provides an implementation of a XRP-specific plugin that sends binary messages over UDP (to account for the less performant hardware on the XRP).

This plugin leverages the work already done for the WebSocket protocol and does a translation to/from JSON/binary.

simulation/halsim_xrp/src/main/native/cpp/HALSimXRP.cpp

@@ -0,0 +1,164 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "HALSimXRP.h"
+
+#include <cstdio>
+
+#include <fmt/format.h>
+#include <wpi/Endian.h>
+#include <wpi/MathExtras.h>
+#include <wpi/SmallString.h>
+#include <wpinet/raw_uv_ostream.h>
+#include <wpinet/uv/util.h>
+
+namespace uv = wpi::uv;
+
+using namespace wpilibxrp;
+
+HALSimXRP::HALSimXRP(wpi::uv::Loop& loop,
+                     wpilibws::ProviderContainer& providers,
+                     wpilibws::HALSimWSProviderSimDevices& simDevicesProvider)
+    : m_loop(loop),
+      m_providers(providers),
+      m_simDevicesProvider(simDevicesProvider) {
+  m_loop.error.connect([](uv::Error err) {
+    fmt::print(stderr, "HALSim XRP Client libuv Error: {}\n", err.str());
+  });
+
+  m_udp_client = uv::Udp::Create(m_loop);
+  m_exec = UvExecFunc::Create(m_loop);
+  if (m_exec) {
+    m_exec->wakeup.connect([](auto func) { func(); });
+  }
+}
+
+bool HALSimXRP::Initialize() {
+  if (!m_udp_client || !m_exec) {
+    return false;
+  }
+
+  const char* host = std::getenv("HALSIMXRP_HOST");
+  if (host != nullptr) {
+    m_host = host;
+  } else {
+    m_host = "localhost";
+  }
+
+  const char* port = std::getenv("HALSIMXRP_PORT");
+  if (port != nullptr) {
+    try {
+      m_port = std::stoi(port);
+    } catch (const std::invalid_argument& err) {
+      fmt::print(stderr, "Error decoding HALSIMXRP_PORT ({})\n", err.what());
+      return false;
+    }
+  } else {
+    m_port = 3540;
+  }
+
+  wpilibxrp::WPILibUpdateFunc func = [&](const wpi::json& data) {
+    OnNetValueChanged(data);
+  };
+
+  m_xrp.SetWPILibUpdateFunc(func);
+
+  return true;
+}
+
+void HALSimXRP::Start() {
+  // struct sockaddr_in dest;
+  uv::NameToAddr(m_host, m_port, &m_dest);
+
+  m_udp_client->Connect(m_dest);
+
+  m_udp_client->received.connect(
+      [this, socket = m_udp_client.get()](auto data, size_t len, auto rinfo,
+                                          unsigned int flags) {
+        ParsePacket({reinterpret_cast<uint8_t*>(data.base), len});
+      });
+
+  m_udp_client->closed.connect([]() { fmt::print("Socket Closed\n"); });
+
+  // Fake the OnNetworkConnected call
+  auto hws = shared_from_this();
+  m_simDevicesProvider.OnNetworkConnected(hws);
+  m_providers.ForEach(
+      [hws](std::shared_ptr<wpilibws::HALSimWSBaseProvider> provider) {
+        provider->OnNetworkConnected(hws);
+      });
+
+  m_udp_client->StartRecv();
+
+  std::puts("HALSimXRP Initialized");
+}
+
+void HALSimXRP::ParsePacket(std::span<const uint8_t> packet) {
+  if (packet.size() < 3) {
+    return;
+  }
+
+  // Hand this off to the XRP object to deal with the messages
+  m_xrp.HandleXRPUpdate(packet);
+}
+
+void HALSimXRP::OnNetValueChanged(const wpi::json& msg) {
+  try {
+    auto& type = msg.at("type").get_ref<const std::string&>();
+    auto& device = msg.at("device").get_ref<const std::string&>();
+
+    wpi::SmallString<64> key;
+    key.append(type);
+    if (!device.empty()) {
+      key.append("/");
+      key.append(device);
+    }
+
+    auto provider = m_providers.Get(key.str());
+    if (provider) {
+      provider->OnNetValueChanged(msg.at("data"));
+    }
+  } catch (wpi::json::exception& e) {
+    fmt::print(stderr, "Error with incoming message: {}\n", e.what());
+  }
+}
+
+void HALSimXRP::OnSimValueChanged(const wpi::json& simData) {
+  // We'll use a signal from robot code to send all the data
+  if (simData["type"] == "HAL") {
+    auto halData = simData["data"];
+    if (halData.find(">sim_periodic_after") != halData.end()) {
+      SendStateToXRP();
+    }
+  } else {
+    m_xrp.HandleWPILibUpdate(simData);
+  }
+}
+
+uv::SimpleBufferPool<4>& HALSimXRP::GetBufferPool() {
+  static uv::SimpleBufferPool<4> bufferPool(128);
+  return bufferPool;
+}
+
+void HALSimXRP::SendStateToXRP() {
+  wpi::SmallVector<uv::Buffer, 4> sendBufs;
+  wpi::raw_uv_ostream stream{sendBufs, [&] {
+                               std::lock_guard lock(m_buffer_mutex);
+                               return GetBufferPool().Allocate();
+                             }};
+  m_xrp.SetupXRPSendBuffer(stream);
+
+  m_exec->Send([this, sendBufs]() mutable {
+    m_udp_client->Send(sendBufs, [&](auto bufs, uv::Error err) {
+      {
+        std::lock_guard lock(m_buffer_mutex);
+        GetBufferPool().Release(bufs);
+      }
+
+      if (err) {
+        // no-op
+      }
+    });
+  });
+}

simulation/halsim_xrp/src/main/native/cpp/HALSimXRPClient.cpp

@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "HALSimXRPClient.h"
+
+#include <WSProviderContainer.h>
+#include <WSProvider_Analog.h>
+#include <WSProvider_BuiltInAccelerometer.h>
+#include <WSProvider_DIO.h>
+#include <WSProvider_DriverStation.h>
+#include <WSProvider_Encoder.h>
+#include <WSProvider_HAL.h>
+#include <WSProvider_SimDevice.h>
+#include <wpinet/EventLoopRunner.h>
+
+using namespace wpilibxrp;
+using namespace wpilibws;
+
+bool HALSimXRPClient::Initialize() {
+  bool result = true;
+  runner.ExecSync([&](wpi::uv::Loop& loop) {
+    simxrp = std::make_shared<HALSimXRP>(loop, providers, simDevices);
+
+    if (!simxrp->Initialize()) {
+      result = false;
+      return;
+    }
+
+    WSRegisterFunc registerFunc = [&](auto key, auto provider) {
+      providers.Add(key, provider);
+    };
+
+    // Minimized set of HAL providers
+    HALSimWSProviderAnalogIn::Initialize(registerFunc);
+    HALSimWSProviderBuiltInAccelerometer::Initialize(registerFunc);
+    HALSimWSProviderDIO::Initialize(registerFunc);
+    HALSimWSProviderDriverStation::Initialize(registerFunc);
+    HALSimWSProviderEncoder::Initialize(registerFunc);
+    HALSimWSProviderHAL::Initialize(registerFunc);
+
+    simDevices.Initialize(loop);
+
+    simxrp->Start();
+  });
+
+  return result;
+}

simulation/halsim_xrp/src/main/native/cpp/XRP.cpp

@@ -0,0 +1,373 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "XRP.h"
+
+#include <fmt/format.h>
+#include <wpi/Endian.h>
+#include <wpi/MathExtras.h>
+
+using namespace wpilibxrp;
+
+XRP::XRP()
+    : m_gyro_name{"XRPGyro"}, m_wpilib_update_func([](const wpi::json&) {}) {
+  // Set up the inputs and outputs
+  m_motor_outputs.emplace(0, 0.0f);
+  m_motor_outputs.emplace(1, 0.0f);
+  m_motor_outputs.emplace(2, 0.0f);
+  m_motor_outputs.emplace(3, 0.0f);
+
+  m_servo_outputs.emplace(4, 0.5f);
+  m_servo_outputs.emplace(5, 0.5f);
+
+  m_encoder_inputs.emplace(1, 0);
+  m_encoder_inputs.emplace(2, 0);
+  m_encoder_inputs.emplace(0, 0);
+  m_encoder_inputs.emplace(3, 0);
+}
+
+void XRP::HandleWPILibUpdate(const wpi::json& data) {
+  if (data.count("type") == 0) {
+    return;
+  }
+
+  if (data["type"] == "DriverStation") {
+    HandleDriverStationSimValueChanged(data);
+  } else if (data["type"] == "XRPMotor") {
+    HandleMotorSimValueChanged(data);
+  } else if (data["type"] == "XRPServo") {
+    HandleServoSimValueChanged(data);
+  } else if (data["type"] == "DIO") {
+    HandleDIOSimValueChanged(data);
+  } else if (data["type"] == "Gyro") {
+    HandleGyroSimValueChanged(data);
+  } else if (data["type"] == "Encoder") {
+    HandleEncoderSimValueChanged(data);
+  }
+}
+
+void XRP::HandleXRPUpdate(std::span<const uint8_t> packet) {
+  uint16_t seq = (packet[0] << 8) + packet[1];
+
+  if (seq <= m_wpilib_bound_seq) {
+    // If the old sequence was within 3 or uint16_t max and the new
+    // sequence is < 3 - we've prob rolled over
+    if (!((0xFFFF - m_wpilib_bound_seq < 3) && seq < 3)) {
+      return;
+    }
+  }
+
+  m_wpilib_bound_seq = seq;
+
+  // Tagged data starts at byte 3
+  packet = packet.subspan(3);
+
+  // Loop to handle multiple tags
+  while (!packet.empty()) {
+    auto tagLength = packet[0];
+    auto tagPacket = packet.subspan(0, tagLength + 1);
+
+    // NOTE: tagPacket contains the size and tag bytes as well
+    // Verify that the packet is indeed the right size
+    if (tagPacket.size() != static_cast<size_t>(tagLength + 1)) {
+      break;
+    }
+
+    switch (packet[1]) {
+      case XRP_TAG_GYRO:
+        ReadGyroTag(tagPacket);
+        break;
+      case XRP_TAG_ACCEL:
+        ReadAccelTag(tagPacket);
+        break;
+      case XRP_TAG_DIO:
+        ReadDIOTag(tagPacket);
+        break;
+      case XRP_TAG_ENCODER:
+        ReadEncoderTag(tagPacket);
+        break;
+      case XRP_TAG_ANALOG:
+        ReadAnalogTag(tagPacket);
+        break;
+    }
+
+    packet = packet.subspan(tagLength + 1);
+  }
+}
+
+void XRP::SetupXRPSendBuffer(wpi::raw_uv_ostream& buf) {
+  SetupSendHeader(buf);
+  SetupMotorTag(buf);
+  SetupServoTag(buf);
+  SetupDigitalOutTag(buf);
+  m_xrp_bound_seq++;
+}
+
+// WPILib Sim Handlers
+void XRP::HandleDriverStationSimValueChanged(const wpi::json& data) {
+  auto dsData = data["data"];
+  if (dsData.find(">enabled") != dsData.end()) {
+    m_robot_enabled = dsData[">enabled"];
+  }
+}
+
+void XRP::HandleMotorSimValueChanged(const wpi::json& data) {
+  int deviceId = -1;
+  auto motorData = data["data"];
+
+  if (data["device"] == "motorL") {
+    deviceId = 0;
+  } else if (data["device"] == "motorR") {
+    deviceId = 1;
+  } else if (data["device"] == "motor3") {
+    deviceId = 2;
+  } else if (data["device"] == "motor4") {
+    deviceId = 3;
+  }
+
+  if (deviceId != -1 && motorData.find("<speed") != motorData.end()) {
+    m_motor_outputs[deviceId] = motorData["<speed"];
+  }
+}
+
+void XRP::HandleServoSimValueChanged(const wpi::json& data) {
+  int deviceId = -1;
+  auto servoData = data["data"];
+
+  if (data["device"] == "servo1") {
+    deviceId = 4;
+  } else if (data["device"] == "servo2") {
+    deviceId = 5;
+  }
+
+  if (deviceId != -1 && servoData.find("<position") != servoData.end()) {
+    m_servo_outputs[deviceId] = servoData["<position"];
+  }
+}
+
+void XRP::HandleDIOSimValueChanged(const wpi::json& data) {
+  int deviceId = -1;
+  auto dioData = data["data"];
+
+  try {
+    deviceId = std::stoi(data["device"].get<std::string>());
+  } catch (const std::invalid_argument&) {
+    deviceId = -1;
+  }
+
+  // Bail out early if device ID is invalid or if it's "spoken for"
+  if (deviceId == -1) {
+    return;
+  }
+
+  if (dioData.find("<init") != dioData.end() && dioData["<init"]) {
+    // All DIOs are initialized as inputs by default
+    m_digital_inputs.emplace(deviceId, false);
+  }
+
+  if (dioData.find("<input") != dioData.end() && dioData["<input"] == false) {
+    // We're registering an output device
+    // Remove from the digital inputs list (if present)
+    m_digital_inputs.erase(deviceId);
+    m_digital_outputs.emplace(deviceId, false);
+  }
+
+  if (dioData.find("<>value") != dioData.end() &&
+      m_digital_outputs.count(deviceId) > 0) {
+    m_digital_outputs[deviceId] = dioData["<>value"];
+  }
+}
+
+void XRP::HandleGyroSimValueChanged(const wpi::json& data) {
+  m_gyro_name = data["device"].get<std::string>();
+}
+
+void XRP::HandleEncoderSimValueChanged(const wpi::json& data) {
+  // We need to handle the various encoder cases
+  // 4/5 -> Encoder 0
+  // 6/7 -> Encoder 1
+  // 8/9 -> Encoder 2
+  // 10/11 -> Encoder 3
+  int deviceId = -1;
+  auto encData = data["data"];
+
+  try {
+    deviceId = std::stoi(data["device"].get<std::string>());
+  } catch (const std::invalid_argument&) {
+    deviceId = -1;
+  }
+
+  if (deviceId == -1) {
+    return;
+  }
+
+  if (encData.find("<init") != encData.end() && encData["<init"]) {
+    // The <channel_a and <channel_b values come with the init message
+    int chA = encData["<channel_a"];
+    int chB = encData["<channel_b"];
+
+    if ((chA == 4 && chB == 5) || (chA == 5 && chB == 4)) {
+      m_encoder_channel_map.emplace(0, deviceId);
+    } else if ((chA == 6 && chB == 7) || (chA == 7 && chB == 6)) {
+      m_encoder_channel_map.emplace(1, deviceId);
+    } else if ((chA == 8 && chB == 9) || (chA == 9 && chB == 8)) {
+      m_encoder_channel_map.emplace(2, deviceId);
+    } else if ((chA == 10 && chB == 11) || (chA == 11 && chB == 10)) {
+      m_encoder_channel_map.emplace(3, deviceId);
+    }
+  }
+}
+
+// ==================================
+// XRP Buffer Generation/Read Methods
+// ==================================
+
+void XRP::SetupSendHeader(wpi::raw_uv_ostream& buf) {
+  uint8_t pktSeq[2];
+  wpi::support::endian::write16be(pktSeq, m_xrp_bound_seq);
+
+  buf << pktSeq[0] << pktSeq[1]
+      << static_cast<uint8_t>(m_robot_enabled ? 1 : 0);
+}
+
+void XRP::SetupMotorTag(wpi::raw_uv_ostream& buf) {
+  uint8_t value[4];
+
+  for (auto motor : m_motor_outputs) {
+    // Motor payload is 6 bytes
+    buf << static_cast<uint8_t>(6)              // Size
+        << static_cast<uint8_t>(XRP_TAG_MOTOR)  // Tag
+        << static_cast<uint8_t>(motor.first);   // Channel
+
+    // Convert the value
+    wpi::support::endian::write32be(value, wpi::FloatToBits(motor.second));
+    buf << value[0] << value[1] << value[2] << value[3];
+  }
+}
+
+void XRP::SetupServoTag(wpi::raw_uv_ostream& buf) {
+  uint8_t value[4];
+
+  for (auto servo : m_servo_outputs) {
+    // Servo payload is 6 bytes
+    buf << static_cast<uint8_t>(6)              // Size
+        << static_cast<uint8_t>(XRP_TAG_SERVO)  // Tag
+        << static_cast<uint8_t>(servo.first);   // Channel
+
+    // Convert the value
+    wpi::support::endian::write32be(value, wpi::FloatToBits(servo.second));
+    buf << value[0] << value[1] << value[2] << value[3];
+  }
+}
+
+void XRP::SetupDigitalOutTag(wpi::raw_uv_ostream& buf) {
+  for (auto digitalOut : m_digital_outputs) {
+    // DIO payload is 3 bytes
+    buf << static_cast<uint8_t>(3)                           // Size
+        << static_cast<uint8_t>(XRP_TAG_DIO)                 // Tag
+        << static_cast<uint8_t>(digitalOut.first)            // Channel
+        << static_cast<uint8_t>(digitalOut.second ? 1 : 0);  // Value
+  }
+}
+
+void XRP::ReadGyroTag(std::span<const uint8_t> packet) {
+  if (packet.size() < 26) {
+    return;  // size(1) + tag(1) + 6x 4byte
+  }
+
+  packet = packet.subspan(2);  // Skip past the size and tag
+  float rate_x = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[0]));
+  float rate_y = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[4]));
+  float rate_z = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[8]));
+  float angle_x = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[12]));
+  float angle_y = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[16]));
+  float angle_z = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[20]));
+
+  // Make the json object
+  wpi::json gyroJson;
+  gyroJson["type"] = "Gyro";
+  gyroJson["device"] = m_gyro_name;
+  gyroJson["data"] = {{">rate_x", rate_x},   {">rate_y", rate_y},
+                      {">rate_z", rate_z},   {">angle_x", angle_x},
+                      {">angle_y", angle_y}, {">angle_z", angle_z}};
+
+  // Update WPILib
+  m_wpilib_update_func(gyroJson);
+}
+
+void XRP::ReadAccelTag(std::span<const uint8_t> packet) {
+  if (packet.size() < 14) {
+    return;  // size(1) + tag(1) + 3x 4 byte
+  }
+
+  packet = packet.subspan(2);  // Skip past the size and tag
+  float accel_x = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[0]));
+  float accel_y = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[4]));
+  float accel_z = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[8]));
+
+  wpi::json accelJson;
+  accelJson["type"] = "Accel";
+  accelJson["device"] = "BuiltInAccel";
+  accelJson["data"] = {{">x", accel_x}, {">y", accel_y}, {">z", accel_z}};
+
+  // Update WPILib
+  m_wpilib_update_func(accelJson);
+}
+
+void XRP::ReadDIOTag(std::span<const uint8_t> packet) {
+  if (packet.size() < 4) {
+    return;  // size(1) + tag(1) + id(1) + value(1)
+  }
+
+  wpi::json dioJson;
+  dioJson["type"] = "DIO";
+  dioJson["device"] = std::to_string(packet[2]);
+  dioJson["data"] = {{"<>value", packet[3] == 1}};
+
+  m_wpilib_update_func(dioJson);
+}
+
+void XRP::ReadEncoderTag(std::span<const uint8_t> packet) {
+  if (packet.size() < 7) {
+    return;  // size(1) + tag(1) + id(1) + value(4)
+  }
+
+  uint8_t encoderId = packet[2];
+
+  packet = packet.subspan(3);  // Skip past the size and tag and ID
+  int32_t value =
+      static_cast<int32_t>(wpi::support::endian::read32be(&packet[0]));
+
+  // Look up the registered encoders
+  if (m_encoder_channel_map.count(encoderId) == 0) {
+    return;
+  }
+
+  uint8_t wpilibEncoderChannel = m_encoder_channel_map[encoderId];
+
+  wpi::json encJson;
+  encJson["type"] = "Encoder";
+  encJson["device"] = std::to_string(wpilibEncoderChannel);
+  encJson["data"] = {{">count", value}};
+
+  m_wpilib_update_func(encJson);
+}
+
+void XRP::ReadAnalogTag(std::span<const uint8_t> packet) {
+  if (packet.size() < 7) {
+    return;  // size(1) + tag(1) + id(1) + float
+  }
+
+  uint8_t analogId = packet[2];
+
+  packet = packet.subspan(3);
+  float voltage = wpi::BitsToFloat(wpi::support::endian::read32be(&packet[0]));
+
+  wpi::json analogJson;
+  analogJson["type"] = "AI";
+  analogJson["device"] = std::to_string(analogId);
+  analogJson["data"] = {{">voltage", voltage}};
+
+  m_wpilib_update_func(analogJson);
+}

simulation/halsim_xrp/src/main/native/cpp/main.cpp

@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cstdio>
+#include <memory>
+
+#include <hal/Extensions.h>
+
+#include "HALSimXRPClient.h"
+
+#if defined(Win32) || defined(_WIN32)
+#pragma comment(lib, "Ws2_32.lib")
+#endif
+
+using namespace wpilibxrp;
+
+static std::unique_ptr<HALSimXRPClient> gClient;
+
+/*--------------------------------------------------------------------------
+** Main Entry Point. Start up the listening threads
+**------------------------------------------------------------------------*/
+extern "C" {
+#if defined(WIN32) || defined(_WIN32)
+__declspec(dllexport)
+#endif
+
+    int HALSIM_InitExtension(void) {
+  std::puts("HALSim XRP Extension Initializing");
+
+  HAL_OnShutdown(nullptr, [](void*) { gClient.reset(); });
+
+  gClient = std::make_unique<HALSimXRPClient>();
+  if (!gClient->Initialize()) {
+    return -1;
+  }
+
+  std::puts("HALSim XRP Extention Initialized");
+  return 0;
+}
+
+}  // extern "C"