[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/include/HALSimXRP.h

@@ -0,0 +1,75 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <span>
+#include <string>
+
+#include <HALSimBaseWebSocketConnection.h>
+#include <WSProviderContainer.h>
+#include <WSProvider_SimDevice.h>
+#include <wpinet/uv/Async.h>
+#include <wpinet/uv/Buffer.h>
+#include <wpinet/uv/Loop.h>
+#include <wpinet/uv/Timer.h>
+#include <wpinet/uv/Udp.h>
+
+#include "XRP.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace wpilibxrp {
+
+// This masquerades as a "WebSocket" so that we can reuse the
+// stuff in halsim_ws_core
+class HALSimXRP : public wpilibws::HALSimBaseWebSocketConnection,
+                  public std::enable_shared_from_this<HALSimXRP> {
+ public:
+  using LoopFunc = std::function<void(void)>;
+  using UvExecFunc = wpi::uv::Async<LoopFunc>;
+
+  HALSimXRP(wpi::uv::Loop& loop, wpilibws::ProviderContainer& providers,
+            wpilibws::HALSimWSProviderSimDevices& simDevicesProvider);
+  HALSimXRP(const HALSimXRP&) = delete;
+  HALSimXRP& operator=(const HALSimXRP&) = delete;
+
+  bool Initialize();
+  void Start();
+
+  void ParsePacket(std::span<const uint8_t> packet);
+  void OnNetValueChanged(const wpi::json& msg);
+  void OnSimValueChanged(const wpi::json& simData) override;
+
+  const std::string& GetTargetHost() const { return m_host; }
+  int GetTargetPort() const { return m_port; }
+  wpi::uv::Loop& GetLoop() { return m_loop; }
+
+  UvExecFunc& GetExec() { return *m_exec; }
+
+ private:
+  XRP m_xrp;
+
+  wpi::uv::Loop& m_loop;
+  std::shared_ptr<wpi::uv::Udp> m_udp_client;
+  std::shared_ptr<UvExecFunc> m_exec;
+
+  wpilibws::ProviderContainer& m_providers;
+  wpilibws::HALSimWSProviderSimDevices& m_simDevicesProvider;
+
+  std::string m_host;
+  int m_port;
+
+  void SendStateToXRP();
+  wpi::uv::SimpleBufferPool<4>& GetBufferPool();
+  std::mutex m_buffer_mutex;
+
+  struct sockaddr_in m_dest;
+};
+
+}  // namespace wpilibxrp

simulation/halsim_xrp/src/main/native/include/HALSimXRPClient.h

@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <WSProviderContainer.h>
+#include <WSProvider_SimDevice.h>
+#include <wpinet/EventLoopRunner.h>
+
+#include "HALSimXRP.h"
+
+namespace wpilibxrp {
+
+class HALSimXRPClient {
+ public:
+  HALSimXRPClient() = default;
+  HALSimXRPClient(const HALSimXRPClient&) = delete;
+  HALSimXRPClient& operator=(const HALSimXRPClient&) = delete;
+
+  bool Initialize();
+
+  wpilibws::ProviderContainer providers;
+  wpilibws::HALSimWSProviderSimDevices simDevices{providers};
+  wpi::EventLoopRunner runner;
+  std::shared_ptr<HALSimXRP> simxrp;
+};
+
+}  // namespace wpilibxrp

simulation/halsim_xrp/src/main/native/include/XRP.h

@@ -0,0 +1,88 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <map>
+#include <span>
+#include <string>
+
+#include <wpi/json.h>
+#include <wpinet/raw_uv_ostream.h>
+
+#define XRP_TAG_MOTOR 0x12
+#define XRP_TAG_SERVO 0x13
+#define XRP_TAG_DIO 0x14
+#define XRP_TAG_ANALOG 0x15
+#define XRP_TAG_GYRO 0x16
+#define XRP_TAG_ACCEL 0x17
+#define XRP_TAG_ENCODER 0x18
+
+namespace wpilibxrp {
+
+using WPILibUpdateFunc = std::function<void(const wpi::json&)>;
+
+class XRP {
+ public:
+  XRP();
+
+  void SetWPILibUpdateFunc(WPILibUpdateFunc func) {
+    m_wpilib_update_func = func;
+  }
+
+  void HandleWPILibUpdate(const wpi::json& data);
+  void HandleXRPUpdate(std::span<const uint8_t> packet);
+
+  void SetupXRPSendBuffer(wpi::raw_uv_ostream& buf);
+
+ private:
+  // To XRP Methods
+  void SetupSendHeader(wpi::raw_uv_ostream& buf);
+  void SetupMotorTag(wpi::raw_uv_ostream& buf);
+  void SetupServoTag(wpi::raw_uv_ostream& buf);
+  void SetupDigitalOutTag(wpi::raw_uv_ostream& buf);
+
+  // WPILib Sim Update Handlers
+  void HandleDriverStationSimValueChanged(const wpi::json& data);
+  void HandleMotorSimValueChanged(const wpi::json& data);
+  void HandleServoSimValueChanged(const wpi::json& data);
+  void HandleDIOSimValueChanged(const wpi::json& data);
+  void HandleGyroSimValueChanged(const wpi::json& data);
+  void HandleEncoderSimValueChanged(const wpi::json& data);
+
+  // XRP Packet Update Handlers
+  void ReadGyroTag(std::span<const uint8_t> packet);
+  void ReadAccelTag(std::span<const uint8_t> packet);
+  void ReadDIOTag(std::span<const uint8_t> packet);
+  void ReadEncoderTag(std::span<const uint8_t> packet);
+  void ReadAnalogTag(std::span<const uint8_t> packet);
+
+  // Robot State
+  std::map<uint8_t, bool> m_digital_outputs;
+  std::map<uint8_t, float> m_motor_outputs;
+  std::map<uint8_t, float> m_servo_outputs;
+
+  // Might not need these
+  std::map<uint8_t, bool> m_digital_inputs;
+  std::map<uint8_t, float> m_analog_inputs;
+  std::map<uint8_t, int32_t> m_encoder_inputs;
+
+  // We need a map from XRP encoder channels (0=left, 1=right etc)
+  // to WPILib device ID
+  // Key: XRP encoder number, Value: WPILib channel
+  // If no encoders are init-ed, this map is empty
+  std::map<uint8_t, uint8_t> m_encoder_channel_map;
+
+  uint16_t m_wpilib_bound_seq = 0;
+  uint16_t m_xrp_bound_seq = 0;
+
+  bool m_robot_enabled = false;
+
+  std::string m_gyro_name;
+
+  WPILibUpdateFunc m_wpilib_update_func;
+};
+
+}  // namespace wpilibxrp