Add DriverStation NetworkTables HAL Extension (#829)

settings.gradle

@@ -11,3 +11,4 @@ include 'myRobot'
 include 'simulation:halsim_print'
 include 'simulation:halsim_lowfi'
 include 'simulation:adx_gyro_accelerometer'
+include 'simulation:halsim_ds_nt'

simulation/halsim_ds_nt/build.gradle

@@ -0,0 +1,58 @@
+description = "A simulation shared object that uses NetworkTables to act as a stand-in for the FRC Driver Station"
+
+apply plugin: 'edu.wpi.first.NativeUtils'
+apply plugin: 'visual-studio'
+apply plugin: 'cpp'
+
+if (!project.hasProperty('onlyAthena')) {
+    ext.skipAthena = true
+
+    apply from: "../../config.gradle"
+
+
+    model {
+        dependencyConfigs {
+            ntcore(DependencyConfig) {
+                groupId = 'edu.wpi.first.ntcore'
+                artifactId = 'ntcore-cpp'
+                headerClassifier = 'headers'
+                ext = 'zip'
+                version = '4.+'
+                sharedConfigs = [ halsim_ds_nt: [] ]
+            }
+            wpiutil(DependencyConfig) {
+                groupId = 'edu.wpi.first.wpiutil'
+                artifactId = 'wpiutil-cpp'
+                headerClassifier = 'headers'
+                ext = 'zip'
+                version = '3.+'
+                sharedConfigs = [ halsim_ds_nt: [] ]
+            }
+        }
+        components {
+            halsim_ds_nt(NativeLibrarySpec) {
+                sources {
+                    cpp {
+                        source {
+                            srcDirs = [ 'src/main/native/cpp' ]
+                            includes = ["**/*.cpp"]
+                        }
+                        exportedHeaders {
+                            srcDirs = ["src/main/native/include"]
+                        }
+                    }
+                }
+            }
+        }
+
+        binaries {
+            all {
+                project(':hal').addHalToLinker(it)
+            }
+            withType(StaticLibraryBinarySpec) {
+                it.buildable = false
+            }
+        }
+    }
+    apply from: 'publish.gradle'
+}

simulation/halsim_ds_nt/publish.gradle

@@ -0,0 +1,94 @@
+apply plugin: 'maven-publish'
+apply plugin: 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin'
+
+if (!hasProperty('releaseType')) {
+    WPILibVersion {
+        releaseType = 'dev'
+    }
+}
+
+def pubVersion = ''
+if (project.hasProperty("publishVersion")) {
+    pubVersion = project.publishVersion
+} else {
+    pubVersion = WPILibVersion.version
+}
+
+def baseArtifactId = 'halsim-ds-nt'
+def artifactGroupId = 'edu.wpi.first.halsim.ds'
+
+def outputsFolder = file("$project.buildDir/outputs")
+
+task cppSourcesZip(type: Zip) {
+    destinationDir = outputsFolder
+    baseName = 'halsim-ds-nt'
+    classifier = "sources"
+
+    from(licenseFile) {
+        into '/'
+    }
+
+    from('src/main/native/cpp') {
+        into '/'
+    }
+}
+
+task cppHeadersZip(type: Zip) {
+    destinationDir = outputsFolder
+    baseName = 'halsim-ds-nt'
+    classifier = "headers"
+
+    from(licenseFile) {
+        into '/'
+    }
+
+    from('src/main/native/include') {
+        into '/'
+    }
+}
+
+build.dependsOn cppSourcesZip
+build.dependsOn cppHeadersZip
+
+
+model {
+    publishing {
+        def pluginTaskList = createComponentZipTasks($.components, 'halsim_ds_nt', 'zipcpp', Zip, project, { task, value->
+            value.each { binary->
+                if (binary.buildable) {
+                    if (binary instanceof SharedLibraryBinarySpec) {
+                        task.dependsOn binary.buildTask
+                        task.from (binary.sharedLibraryFile) {
+                            into getPlatformPath(binary) + '/shared'
+                        }
+                    }
+                }
+            }
+        })
+
+        def allTask
+        if (!project.hasProperty('jenkinsBuild')) {
+            allTask = createAllCombined(pluginTaskList, 'halsim_ds_nt', 'zipcpp', Zip, project)
+        }
+
+        publications {
+            cpp(MavenPublication) {
+                pluginTaskList.each {
+                    artifact it
+                }
+
+                if (!project.hasProperty('jenkinsBuild')) {
+                    artifact allTask
+                }
+
+                artifact cppHeadersZip
+                artifact cppSourcesZip
+
+
+                artifactId = baseArtifactId
+                groupId artifactGroupId
+                version pubVersion
+            }
+        }
+    }
+}

simulation/halsim_ds_nt/src/main/native/cpp/HALSimDsNt.cpp

@@ -0,0 +1,194 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 "HALSimDsNt.h"
+
+void HALSimDSNT::Initialize() {
+  rootTable =
+      nt::NetworkTableInstance::GetDefault().GetTable("sim")->GetSubTable(
+          "DS_CONTROL");  // Not to be confused with sim::DriverStation from
+                          // HALSim LowFi
+
+  // LOOP TIMING //
+
+  auto timinghz = rootTable->GetEntry("timing_hz");
+  timinghz.ForceSetDouble(50);
+  timinghz.AddListener(
+      [this](const nt::EntryNotification& ev) -> void {
+        double valIn = ev.value->GetDouble();
+        double val = 0;
+        val = (valIn < 1 ? 1 : valIn > 100 ? 100 : valIn);
+
+        if (val != valIn) {
+          this->rootTable->GetEntry("timing_hz").ForceSetDouble(val);
+          Flush();
+        }
+
+        this->timingHz = val;
+      },
+      NT_NotifyKind::NT_NOTIFY_UPDATE);
+
+  // MODES //
+
+  modeTable = rootTable->GetSubTable("mode");
+  auto mtele = modeTable->GetEntry("teleop?");
+  auto mauto = modeTable->GetEntry("auto?");
+  auto mtest = modeTable->GetEntry("test?");
+  auto enabled = modeTable->GetEntry("enabled?");
+  auto estop = modeTable->GetEntry("estop?");
+
+  mtele.ForceSetBoolean(true);
+  mauto.ForceSetBoolean(false);
+  mtest.ForceSetBoolean(false);
+  enabled.ForceSetBoolean(false);
+  estop.ForceSetBoolean(false);
+
+  mtele.AddListener(
+      [this](const nt::EntryNotification& ev) -> void {
+        this->HandleModePress(HALSimDSNT_Mode::teleop, ev.value->GetBoolean());
+      },
+      NT_NotifyKind::NT_NOTIFY_UPDATE);
+
+  mauto.AddListener(
+      [this](const nt::EntryNotification& ev) -> void {
+        this->HandleModePress(HALSimDSNT_Mode::auton, ev.value->GetBoolean());
+      },
+      NT_NotifyKind::NT_NOTIFY_UPDATE);
+
+  mtest.AddListener(
+      [this](const nt::EntryNotification& ev) -> void {
+        this->HandleModePress(HALSimDSNT_Mode::test, ev.value->GetBoolean());
+      },
+      NT_NotifyKind::NT_NOTIFY_UPDATE);
+
+  enabled.AddListener(
+      [this](const nt::EntryNotification& ev) -> void {
+        std::lock_guard<wpi::mutex> lock(modeMutex);
+        if (!this->isEstop) {
+          this->isEnabled = ev.value->GetBoolean();
+        } else {
+          this->isEnabled = false;
+        }
+        this->DoModeUpdate();
+        this->UpdateModeButtons();
+      },
+      NT_NotifyKind::NT_NOTIFY_UPDATE);
+
+  estop.AddListener(
+      [this](const nt::EntryNotification& ev) -> void {
+        std::lock_guard<wpi::mutex> lock(modeMutex);
+        this->isEstop = ev.value->GetBoolean();
+        if (this->isEstop) {
+          this->isEnabled = false;
+        }
+        this->DoModeUpdate();
+        this->UpdateModeButtons();
+      },
+      NT_NotifyKind::NT_NOTIFY_UPDATE);
+
+  // ALLIANCES //
+
+  allianceTable = rootTable->GetSubTable("alliance");
+  auto allianceStation = allianceTable->GetEntry("station");
+  auto allianceColorRed = allianceTable->GetEntry("red?");
+
+  allianceStation.ForceSetDouble(1);
+  allianceColorRed.ForceSetBoolean(true);
+
+  allianceStation.AddListener(
+      [this](const nt::EntryNotification& ev) -> void {
+        double stnIn = ev.value->GetDouble();
+        double stn = 0;
+        stn = (stnIn > 3 ? 3 : stnIn < 1 ? 1 : stnIn);
+
+        if (stn != stnIn) {
+          this->allianceTable->GetEntry("station").ForceSetDouble(stn);
+          Flush();
+        }
+
+        this->allianceStation = stn;
+        this->DoAllianceUpdate();
+      },
+      NT_NotifyKind::NT_NOTIFY_UPDATE);
+
+  allianceColorRed.AddListener(
+      [this](const nt::EntryNotification& ev) -> void {
+        this->isAllianceRed = ev.value->GetBoolean();
+        this->DoAllianceUpdate();
+      },
+      NT_NotifyKind::NT_NOTIFY_UPDATE);
+
+  // FINAL LOGIC //
+
+  Flush();
+
+  loopThread = std::thread([this]() -> void {
+    this->running = true;
+    this->LoopFunc();
+  });
+  loopThread.detach();
+}
+
+void HALSimDSNT::HandleModePress(enum HALSimDSNT_Mode mode, bool isPressed) {
+  if (isPressed) {
+    if (mode != currentMode) {
+      std::lock_guard<wpi::mutex> lock(modeMutex);
+      currentMode = mode;
+      isEnabled = false;
+      this->DoModeUpdate();
+    }
+  }
+
+  this->UpdateModeButtons();
+}
+
+void HALSimDSNT::UpdateModeButtons() {
+  modeTable->GetEntry("teleop?").ForceSetBoolean(currentMode ==
+                                                 HALSimDSNT_Mode::teleop);
+  modeTable->GetEntry("auto?").ForceSetBoolean(currentMode ==
+                                               HALSimDSNT_Mode::auton);
+  modeTable->GetEntry("test?").ForceSetBoolean(currentMode ==
+                                               HALSimDSNT_Mode::test);
+  modeTable->GetEntry("enabled?").ForceSetBoolean(isEnabled);
+  Flush();
+}
+
+void HALSimDSNT::DoModeUpdate() {
+  HALSIM_SetDriverStationAutonomous(currentMode == HALSimDSNT_Mode::auton);
+  HALSIM_SetDriverStationTest(currentMode == HALSimDSNT_Mode::test);
+  HALSIM_SetDriverStationEnabled(isEnabled);
+  if (isEnabled && !lastIsEnabled) {
+    currentMatchTime = 0;
+  }
+  lastIsEnabled = isEnabled;
+  HALSIM_SetDriverStationEStop(isEstop);
+  HALSIM_SetDriverStationFmsAttached(false);
+  HALSIM_SetDriverStationDsAttached(true);
+  HALSIM_NotifyDriverStationNewData();
+}
+
+void HALSimDSNT::DoAllianceUpdate() {
+  HALSIM_SetDriverStationAllianceStationId(static_cast<HAL_AllianceStationID>(
+      (isAllianceRed ? HAL_AllianceStationID_kRed1
+                     : HAL_AllianceStationID_kBlue1) +
+      (static_cast<int32_t>(allianceStation) - 1)));
+}
+
+void HALSimDSNT::LoopFunc() {
+  while (running) {
+    double dt = 1000 / timingHz;
+    std::this_thread::sleep_for(
+        std::chrono::milliseconds(static_cast<int64_t>(dt)));
+    if (isEnabled) {
+      currentMatchTime = currentMatchTime + dt;
+      HALSIM_SetDriverStationMatchTime(currentMatchTime);
+    }
+    HALSIM_NotifyDriverStationNewData();
+  }
+}
+
+void HALSimDSNT::Flush() { rootTable->GetInstance().Flush(); }

simulation/halsim_ds_nt/src/main/native/cpp/main.cpp

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <iostream>
+
+#include <HALSimDsNt.h>
+
+static HALSimDSNT dsnt;
+
+extern "C" {
+#if defined(WIN32) || defined(_WIN32)
+__declspec(dllexport)
+#endif
+    int HALSIM_InitExtension(void) {
+  std::cout << "DriverStationNT Initializing." << std::endl;
+
+  dsnt.Initialize();
+
+  std::cout << "DriverStationNT Initialized!" << std::endl;
+  return 0;
+}
+}  // extern "C"

simulation/halsim_ds_nt/src/main/native/include/HALSimDsNt.h

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 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 <atomic>
+#include <memory>
+#include <thread>
+
+#include <MockData/DriverStationData.h>
+#include <networktables/NetworkTableInstance.h>
+#include <support/mutex.h>
+
+enum HALSimDSNT_Mode { teleop, auton, test };
+
+class HALSimDSNT {
+ public:
+  std::shared_ptr<nt::NetworkTable> rootTable, modeTable, allianceTable;
+  enum HALSimDSNT_Mode currentMode;
+  bool isEnabled, lastIsEnabled, isEstop;
+  std::atomic<bool> isAllianceRed, running;
+  std::atomic<double> currentMatchTime, timingHz, allianceStation;
+  std::thread loopThread;
+  wpi::mutex modeMutex;
+
+  void Initialize();
+  void HandleModePress(enum HALSimDSNT_Mode mode, bool isPressed);
+  void UpdateModeButtons();
+  void DoModeUpdate();
+  void DoAllianceUpdate();
+  void LoopFunc();
+  void Flush();
+};