[sim] Add Mechanism2D to Simulation GUI (#2607)

wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "frc/simulation/Mechanism2D.h"
+
+#include <wpi/SmallString.h>
+#include <wpi/Twine.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+Mechanism2D::Mechanism2D() : m_device{"Mechanism2D"} {}
+
+void Mechanism2D::SetLigamentAngle(const wpi::Twine& ligamentPath,
+                                   float angle) {
+  if (m_device) {
+    wpi::SmallString<64> fullPathBuf;
+    wpi::StringRef fullPath =
+        (ligamentPath + "/angle").toNullTerminatedStringRef(fullPathBuf);
+    if (!createdItems.count(fullPath)) {
+      createdItems[fullPath] =
+          m_device.CreateDouble(fullPath.data(), false, angle);
+    }
+    createdItems[fullPath].Set(angle);
+  }
+}
+
+void Mechanism2D::SetLigamentLength(const wpi::Twine& ligamentPath,
+                                    float length) {
+  if (m_device) {
+    wpi::SmallString<64> fullPathBuf;
+    wpi::StringRef fullPath =
+        (ligamentPath + "/length").toNullTerminatedStringRef(fullPathBuf);
+    if (!createdItems.count(fullPath)) {
+      createdItems[fullPath] =
+          m_device.CreateDouble(fullPath.data(), false, length);
+    }
+    createdItems[fullPath].Set(length);
+  }
+}

wpilibc/src/main/native/include/frc/simulation/Mechanism2D.h

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <map>
+#include <string>
+
+#include <hal/SimDevice.h>
+#include <wpi/StringMap.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+class Mechanism2D {
+ public:
+  Mechanism2D();
+
+  /**
+   * Set/Create the angle of a ligament
+   *
+   * @param ligamentPath json path to the ligament
+   * @param angle to set the ligament
+   */
+  void SetLigamentAngle(const wpi::Twine& ligamentPath, float angle);
+
+  /**
+   * Set/Create the length of a ligament
+   *
+   * @param ligamentPath json path to the ligament
+   * @param length to set the ligament
+   */
+  void SetLigamentLength(const wpi::Twine& ligamentPath, float length);
+
+ private:
+  wpi::StringMap<hal::SimDouble> createdItems;
+  hal::SimDevice m_device;
+};
+
+}  // namespace frc