[glass, wpilib] Rewrite Mechanism2d (#3281)

Substantially improves Mechanism2d by moving it to NetworkTables and adding
a robot API to create the mechanism elements, instead of requiring a JSON file.

Co-authored-by: Peter Johnson <johnson.peter@gmail.com>

wpilibc/src/main/native/cpp/smartdashboard/Mechanism2d.cpp

@@ -0,0 +1,55 @@
+// 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 "frc/smartdashboard/Mechanism2d.h"
+
+#include <cstdio>
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+static constexpr char kBackgroundColor[] = "backgroundColor";
+static constexpr char kDims[] = "dims";
+
+Mechanism2d::Mechanism2d(double width, double height,
+                         const Color8Bit& backgroundColor)
+    : m_width{width}, m_height{height} {
+  SetBackgroundColor(backgroundColor);
+}
+
+MechanismRoot2d* Mechanism2d::GetRoot(wpi::StringRef name, double x, double y) {
+  auto& obj = m_roots[name];
+  if (obj) {
+    return obj.get();
+  }
+  obj = std::make_unique<MechanismRoot2d>(name, x, y,
+                                          MechanismRoot2d::private_init{});
+  if (m_table) {
+    obj->Update(m_table->GetSubTable(name));
+  }
+  return obj.get();
+}
+
+void Mechanism2d::SetBackgroundColor(const Color8Bit& color) {
+  std::snprintf(m_color, sizeof(m_color), "#%02X%02X%02X", color.red,
+                color.green, color.blue);
+  if (m_table) {
+    m_table->GetEntry(kBackgroundColor).SetString(m_color);
+  }
+}
+
+void Mechanism2d::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Mechanism2d");
+  m_table = builder.GetTable();
+
+  m_table->GetEntry(kDims).SetDoubleArray({m_width, m_height});
+  m_table->GetEntry(kBackgroundColor).SetString(m_color);
+
+  std::scoped_lock lock(m_mutex);
+  for (const auto& entry : m_roots) {
+    const auto& root = entry.getValue().get();
+    root->Update(m_table->GetSubTable(entry.getKey()));
+  }
+}

wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp

@@ -0,0 +1,80 @@
+// 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 "frc/smartdashboard/MechanismLigament2d.h"
+
+#include <cstdio>
+
+using namespace frc;
+
+MechanismLigament2d::MechanismLigament2d(const wpi::Twine& name, double length,
+                                         units::degree_t angle,
+                                         double lineWeight,
+                                         const frc::Color8Bit& color)
+    : MechanismObject2d(name),
+      m_length{length},
+      m_angle{angle.to<double>()},
+      m_weight{lineWeight} {
+  SetColor(color);
+}
+
+void MechanismLigament2d::UpdateEntries(std::shared_ptr<NetworkTable> table) {
+  table->GetEntry(".type").SetString("line");
+
+  m_colorEntry = table->GetEntry("color");
+  m_angleEntry = table->GetEntry("angle");
+  m_weightEntry = table->GetEntry("weight");
+  m_lengthEntry = table->GetEntry("length");
+  Flush();
+}
+
+void MechanismLigament2d::SetColor(const Color8Bit& color) {
+  std::scoped_lock lock(m_mutex);
+  std::snprintf(m_color, sizeof(m_color), "#%02X%02X%02X", color.red,
+                color.green, color.blue);
+  Flush();
+}
+
+void MechanismLigament2d::SetAngle(units::degree_t angle) {
+  std::scoped_lock lock(m_mutex);
+  m_angle = angle.to<double>();
+  Flush();
+}
+
+void MechanismLigament2d::SetLineWeight(double lineWidth) {
+  std::scoped_lock lock(m_mutex);
+  m_weight = lineWidth;
+  Flush();
+}
+
+double MechanismLigament2d::GetAngle() {
+  if (m_angleEntry) {
+    m_angle = m_angleEntry.GetDouble(0.0);
+  }
+  return m_angle;
+}
+
+double MechanismLigament2d::GetLength() {
+  if (m_lengthEntry) {
+    m_length = m_lengthEntry.GetDouble(0.0);
+  }
+  return m_length;
+}
+
+void MechanismLigament2d::SetLength(double length) {
+  std::scoped_lock lock(m_mutex);
+  m_length = length;
+  Flush();
+}
+
+#define SAFE_WRITE(data, Type)           \
+  if (m_##data##Entry) {                 \
+    m_##data##Entry.Set##Type(m_##data); \
+  }
+void MechanismLigament2d::Flush() {
+  SAFE_WRITE(color, String)
+  SAFE_WRITE(angle, Double)
+  SAFE_WRITE(length, Double)
+  SAFE_WRITE(weight, Double)
+}

wpilibc/src/main/native/cpp/smartdashboard/MechanismObject2d.cpp

@@ -0,0 +1,24 @@
+// 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 "frc/smartdashboard/MechanismObject2d.h"
+
+using namespace frc;
+
+MechanismObject2d::MechanismObject2d(const wpi::Twine& name)
+    : m_name{name.str()} {}
+
+const std::string& MechanismObject2d::GetName() const {
+  return m_name;
+}
+
+void MechanismObject2d::Update(std::shared_ptr<NetworkTable> table) {
+  std::scoped_lock lock(m_mutex);
+  m_table = table;
+  UpdateEntries(m_table);
+  for (const wpi::StringMapEntry<std::unique_ptr<MechanismObject2d>>& entry :
+       m_objects) {
+    entry.getValue()->Update(m_table->GetSubTable(entry.getKey()));
+  }
+}

wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp

@@ -0,0 +1,35 @@
+// 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 "frc/smartdashboard/MechanismRoot2d.h"
+
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+#include "frc/util/Color8Bit.h"
+
+using namespace frc;
+
+static constexpr char kPosition[] = "pos";
+
+MechanismRoot2d::MechanismRoot2d(const wpi::Twine& name, double x, double y,
+                                 const private_init&)
+    : MechanismObject2d(name.str()), m_x{x}, m_y{y} {}
+
+void MechanismRoot2d::SetPosition(double x, double y) {
+  std::scoped_lock lock(m_mutex);
+  m_x = x;
+  m_y = y;
+  Flush();
+}
+
+void MechanismRoot2d::UpdateEntries(std::shared_ptr<NetworkTable> table) {
+  m_posEntry = table->GetEntry(kPosition);
+  Flush();
+}
+
+inline void MechanismRoot2d::Flush() {
+  if (m_posEntry) {
+    m_posEntry.SetDoubleArray({m_x, m_y});
+  }
+}