[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>

glass/src/lib/native/cpp/other/Mechanism2D.cpp

@@ -0,0 +1,257 @@
+// 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 "glass/other/Mechanism2D.h"
+
+#include <algorithm>
+#include <cmath>
+#include <memory>
+#include <utility>
+
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Transform2d.h>
+#include <frc/geometry/Translation2d.h>
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <imgui_stdlib.h>
+#include <portable-file-dialogs.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <wpi/raw_ostream.h>
+#include <wpigui.h>
+
+#include "glass/Context.h"
+
+using namespace glass;
+
+namespace gui = wpi::gui;
+
+namespace {
+
+// Per-frame data (not persistent)
+struct FrameData {
+  frc::Translation2d GetPosFromScreen(const ImVec2& cursor) const {
+    return {
+        units::meter_t{(std::clamp(cursor.x, min.x, max.x) - min.x) / scale},
+        units::meter_t{(max.y - std::clamp(cursor.y, min.y, max.y)) / scale}};
+  }
+  ImVec2 GetScreenFromPos(const frc::Translation2d& pos) const {
+    return {min.x + scale * pos.X().to<float>(),
+            max.y - scale * pos.Y().to<float>()};
+  }
+  void DrawObject(ImDrawList* drawList, MechanismObjectModel& objModel,
+                  const frc::Pose2d& pose) const;
+  void DrawGroup(ImDrawList* drawList, MechanismObjectGroup& group,
+                 const frc::Pose2d& pose) const;
+
+  // in screen coordinates
+  ImVec2 imageMin;
+  ImVec2 imageMax;
+  ImVec2 min;
+  ImVec2 max;
+
+  float scale;  // scaling from meters to screen units
+};
+
+class BackgroundInfo {
+ public:
+  BackgroundInfo();
+
+  void DisplaySettings();
+
+  void LoadImage();
+  FrameData GetFrameData(ImVec2 min, ImVec2 max, frc::Translation2d dims) const;
+  void Draw(ImDrawList* drawList, const FrameData& frameData,
+            ImU32 bgColor) const;
+
+ private:
+  void Reset();
+  bool LoadImageImpl(const char* fn);
+
+  std::unique_ptr<pfd::open_file> m_fileOpener;
+
+  std::string* m_pFilename;
+  gui::Texture m_texture;
+
+  // in image pixels
+  int m_imageWidth;
+  int m_imageHeight;
+};
+
+}  // namespace
+
+BackgroundInfo::BackgroundInfo() {
+  auto& storage = GetStorage();
+  m_pFilename = storage.GetStringRef("image");
+}
+
+void BackgroundInfo::DisplaySettings() {
+  if (ImGui::Button("Choose image...")) {
+    m_fileOpener = std::make_unique<pfd::open_file>(
+        "Choose background image", "",
+        std::vector<std::string>{"Image File",
+                                 "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
+                                 "*.hdr *.pic *.ppm *.pgm"});
+  }
+  if (ImGui::Button("Reset background image")) {
+    Reset();
+  }
+}
+
+void BackgroundInfo::Reset() {
+  m_texture = gui::Texture{};
+  m_pFilename->clear();
+  m_imageWidth = 0;
+  m_imageHeight = 0;
+}
+
+void BackgroundInfo::LoadImage() {
+  if (m_fileOpener && m_fileOpener->ready(0)) {
+    auto result = m_fileOpener->result();
+    if (!result.empty()) {
+      LoadImageImpl(result[0].c_str());
+    }
+    m_fileOpener.reset();
+  }
+  if (!m_texture && !m_pFilename->empty()) {
+    if (!LoadImageImpl(m_pFilename->c_str())) {
+      m_pFilename->clear();
+    }
+  }
+}
+
+bool BackgroundInfo::LoadImageImpl(const char* fn) {
+  wpi::outs() << "GUI: loading background image '" << fn << "'\n";
+  auto texture = gui::Texture::CreateFromFile(fn);
+  if (!texture) {
+    wpi::errs() << "GUI: could not read background image\n";
+    return false;
+  }
+  m_texture = std::move(texture);
+  m_imageWidth = m_texture.GetWidth();
+  m_imageHeight = m_texture.GetHeight();
+  *m_pFilename = fn;
+  return true;
+}
+
+FrameData BackgroundInfo::GetFrameData(ImVec2 min, ImVec2 max,
+                                       frc::Translation2d dims) const {
+  // fit the image into the window
+  if (m_texture && m_imageHeight != 0 && m_imageWidth != 0) {
+    gui::MaxFit(&min, &max, m_imageWidth, m_imageHeight);
+  }
+
+  FrameData frameData;
+  frameData.imageMin = min;
+  frameData.imageMax = max;
+
+  // determine the "active area"
+  float width = dims.X().to<float>();
+  float height = dims.Y().to<float>();
+  gui::MaxFit(&min, &max, width, height);
+
+  frameData.min = min;
+  frameData.max = max;
+  frameData.scale = (max.x - min.x) / width;
+  return frameData;
+}
+
+void BackgroundInfo::Draw(ImDrawList* drawList, const FrameData& frameData,
+                          ImU32 bgColor) const {
+  if (m_texture && m_imageHeight != 0 && m_imageWidth != 0) {
+    drawList->AddImage(m_texture, frameData.imageMin, frameData.imageMax);
+  } else {
+    drawList->AddRectFilled(frameData.min, frameData.max, bgColor);
+  }
+}
+
+void glass::DisplayMechanism2DSettings(Mechanism2DModel* model) {
+  auto& storage = GetStorage();
+  auto bg = storage.GetData<BackgroundInfo>();
+  if (!bg) {
+    storage.SetData(std::make_shared<BackgroundInfo>());
+    bg = storage.GetData<BackgroundInfo>();
+  }
+  bg->DisplaySettings();
+}
+
+void FrameData::DrawObject(ImDrawList* drawList, MechanismObjectModel& objModel,
+                           const frc::Pose2d& pose) const {
+  const char* type = objModel.GetType();
+  if (wpi::StringRef{type} == "line") {
+    auto startPose =
+        pose + frc::Transform2d{frc::Translation2d{}, objModel.GetAngle()};
+    auto endPose =
+        startPose +
+        frc::Transform2d{frc::Translation2d{objModel.GetLength(), 0_m}, 0_deg};
+    drawList->AddLine(GetScreenFromPos(startPose.Translation()),
+                      GetScreenFromPos(endPose.Translation()),
+                      objModel.GetColor(), objModel.GetWeight());
+    DrawGroup(drawList, objModel, endPose);
+  }
+}
+
+void FrameData::DrawGroup(ImDrawList* drawList, MechanismObjectGroup& group,
+                          const frc::Pose2d& pose) const {
+  group.ForEachObject(
+      [&](auto& objModel) { DrawObject(drawList, objModel, pose); });
+}
+
+void glass::DisplayMechanism2D(Mechanism2DModel* model,
+                               const ImVec2& contentSize) {
+  auto& storage = GetStorage();
+  auto bg = storage.GetData<BackgroundInfo>();
+  if (!bg) {
+    storage.SetData(std::make_shared<BackgroundInfo>());
+    bg = storage.GetData<BackgroundInfo>();
+  }
+
+  if (contentSize.x <= 0 || contentSize.y <= 0) {
+    return;
+  }
+
+  // screen coords
+  ImVec2 cursorPos = ImGui::GetWindowPos() + ImGui::GetCursorPos();
+
+  ImGui::InvisibleButton("background", contentSize);
+
+  // auto mousePos = ImGui::GetIO().MousePos;
+  auto drawList = ImGui::GetWindowDrawList();
+  // bool isHovered = ImGui::IsItemHovered();
+
+  // background
+  bg->LoadImage();
+  auto frameData = bg->GetFrameData(cursorPos, cursorPos + contentSize,
+                                    model->GetDimensions());
+  bg->Draw(drawList, frameData, model->GetBackgroundColor());
+
+  // elements
+  model->ForEachRoot([&](auto& rootModel) {
+    frameData.DrawGroup(drawList, rootModel,
+                        frc::Pose2d{rootModel.GetPosition(), 0_deg});
+  });
+
+#if 0
+  if (target) {
+    // show tooltip and highlight
+    auto pos = frameData.GetPosFromScreen(target->poseCenter);
+    ImGui::SetTooltip(
+        "%s[%d]\nx: %0.3f y: %0.3f rot: %0.3f", target->name.c_str(),
+        static_cast<int>(target->index), ConvertDisplayLength(pos.X()),
+        ConvertDisplayLength(pos.Y()), ConvertDisplayAngle(target->rot));
+  }
+#endif
+}
+
+void Mechanism2DView::Display() {
+  if (ImGui::BeginPopupContextItem()) {
+    DisplayMechanism2DSettings(m_model);
+    ImGui::EndPopup();
+  }
+  DisplayMechanism2D(m_model, ImGui::GetWindowContentRegionMax() -
+                                  ImGui::GetWindowContentRegionMin());
+}

glass/src/lib/native/include/glass/other/Mechanism2D.h

@@ -0,0 +1,63 @@
+// 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 <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
+#include <imgui.h>
+#include <wpi/STLExtras.h>
+
+#include "glass/Model.h"
+#include "glass/View.h"
+
+namespace glass {
+
+class MechanismObjectModel;
+
+class MechanismObjectGroup {
+ public:
+  virtual const char* GetName() const = 0;
+  virtual void ForEachObject(
+      wpi::function_ref<void(MechanismObjectModel& model)> func) = 0;
+};
+
+class MechanismObjectModel : public MechanismObjectGroup {
+ public:
+  virtual const char* GetType() const = 0;
+  virtual ImU32 GetColor() const = 0;
+
+  // line accessors
+  virtual double GetWeight() const = 0;
+  virtual frc::Rotation2d GetAngle() const = 0;
+  virtual units::meter_t GetLength() const = 0;
+};
+
+class MechanismRootModel : public MechanismObjectGroup {
+ public:
+  virtual frc::Translation2d GetPosition() const = 0;
+};
+
+class Mechanism2DModel : public Model {
+ public:
+  virtual frc::Translation2d GetDimensions() const = 0;
+  virtual ImU32 GetBackgroundColor() const = 0;
+  virtual void ForEachRoot(
+      wpi::function_ref<void(MechanismRootModel& model)> func) = 0;
+};
+
+void DisplayMechanism2D(Mechanism2DModel* model, const ImVec2& contentSize);
+void DisplayMechanism2DSettings(Mechanism2DModel* model);
+
+class Mechanism2DView : public View {
+ public:
+  explicit Mechanism2DView(Mechanism2DModel* model) : m_model{model} {}
+
+  void Display() override;
+
+ private:
+  Mechanism2DModel* m_model;
+};
+
+}  // namespace glass

glass/src/libnt/native/cpp/NTMechanism2D.cpp

@@ -0,0 +1,297 @@
+// 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 "glass/networktables/NTMechanism2D.h"
+
+#include <algorithm>
+#include <vector>
+
+#include <imgui.h>
+#include <ntcore_cpp.h>
+
+#include "glass/other/Mechanism2D.h"
+
+using namespace glass;
+
+// Convert "#RRGGBB" hex color to ImU32 color
+static void ConvertColor(wpi::StringRef in, ImU32* out) {
+  if (in.size() != 7 || in[0] != '#') {
+    return;
+  }
+  ImU32 val = 0;
+  if (in.drop_front().getAsInteger(16, val)) {
+    return;
+  }
+  *out = IM_COL32((val >> 16) & 0xff, (val >> 8) & 0xff, val & 0xff, 255);
+}
+
+namespace {
+
+class NTMechanismObjectModel;
+
+class NTMechanismGroupImpl final {
+ public:
+  NTMechanismGroupImpl(NT_Inst inst, const wpi::Twine& path,
+                       wpi::StringRef name)
+      : m_inst{inst}, m_path{path.str()}, m_name{name} {}
+
+  const char* GetName() const { return m_name.c_str(); }
+  void ForEachObject(wpi::function_ref<void(MechanismObjectModel& model)> func);
+  void NTUpdate(const nt::EntryNotification& event, wpi::StringRef name);
+
+ protected:
+  NT_Inst m_inst;
+  std::string m_path;
+  std::string m_name;
+  std::vector<std::unique_ptr<NTMechanismObjectModel>> m_objects;
+};
+
+class NTMechanismObjectModel final : public MechanismObjectModel {
+ public:
+  NTMechanismObjectModel(NT_Inst inst, const wpi::Twine& path,
+                         wpi::StringRef name)
+      : m_group{inst, path, name},
+        m_type{nt::GetEntry(inst, path + "/.type")},
+        m_color{nt::GetEntry(inst, path + "/color")},
+        m_weight{nt::GetEntry(inst, path + "/weight")},
+        m_angle{nt::GetEntry(inst, path + "/angle")},
+        m_length{nt::GetEntry(inst, path + "/length")} {}
+
+  const char* GetName() const final { return m_group.GetName(); }
+  void ForEachObject(
+      wpi::function_ref<void(MechanismObjectModel& model)> func) final {
+    m_group.ForEachObject(func);
+  }
+
+  const char* GetType() const final { return m_typeValue.c_str(); }
+  ImU32 GetColor() const final { return m_colorValue; }
+  double GetWeight() const final { return m_weightValue; }
+  frc::Rotation2d GetAngle() const final { return m_angleValue; }
+  units::meter_t GetLength() const final { return m_lengthValue; }
+
+  bool NTUpdate(const nt::EntryNotification& event, wpi::StringRef childName);
+
+ private:
+  NTMechanismGroupImpl m_group;
+
+  NT_Entry m_type;
+  NT_Entry m_color;
+  NT_Entry m_weight;
+  NT_Entry m_angle;
+  NT_Entry m_length;
+
+  std::string m_typeValue;
+  ImU32 m_colorValue = IM_COL32_WHITE;
+  double m_weightValue = 1.0;
+  frc::Rotation2d m_angleValue;
+  units::meter_t m_lengthValue = 0.0_m;
+};
+
+}  // namespace
+
+void NTMechanismGroupImpl::ForEachObject(
+    wpi::function_ref<void(MechanismObjectModel& model)> func) {
+  for (auto&& obj : m_objects) {
+    func(*obj);
+  }
+}
+
+void NTMechanismGroupImpl::NTUpdate(const nt::EntryNotification& event,
+                                    wpi::StringRef name) {
+  if (name.empty()) {
+    return;
+  }
+  wpi::StringRef childName;
+  std::tie(name, childName) = name.split('/');
+  if (childName.empty()) {
+    return;
+  }
+
+  auto it = std::lower_bound(
+      m_objects.begin(), m_objects.end(), name,
+      [](const auto& e, wpi::StringRef name) { return e->GetName() < name; });
+  bool match = it != m_objects.end() && (*it)->GetName() == name;
+
+  if (event.flags & NT_NOTIFY_NEW) {
+    if (!match) {
+      it = m_objects.emplace(it, std::make_unique<NTMechanismObjectModel>(
+                                     m_inst, m_path + "/" + name, name));
+      match = true;
+    }
+  }
+  if (match) {
+    if ((*it)->NTUpdate(event, childName)) {
+      m_objects.erase(it);
+    }
+  }
+}
+
+bool NTMechanismObjectModel::NTUpdate(const nt::EntryNotification& event,
+                                      wpi::StringRef childName) {
+  if (event.entry == m_type) {
+    if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+      return true;
+    }
+    if (event.value && event.value->IsString()) {
+      m_typeValue = event.value->GetString();
+    }
+  } else if (event.entry == m_color) {
+    if (event.value && event.value->IsString()) {
+      ConvertColor(event.value->GetString(), &m_colorValue);
+    }
+  } else if (event.entry == m_weight) {
+    if (event.value && event.value->IsDouble()) {
+      m_weightValue = event.value->GetDouble();
+    }
+  } else if (event.entry == m_angle) {
+    if (event.value && event.value->IsDouble()) {
+      m_angleValue = units::degree_t{event.value->GetDouble()};
+    }
+  } else if (event.entry == m_length) {
+    if (event.value && event.value->IsDouble()) {
+      m_lengthValue = units::meter_t{event.value->GetDouble()};
+    }
+  } else {
+    m_group.NTUpdate(event, childName);
+  }
+  return false;
+}
+
+class NTMechanism2DModel::RootModel final : public MechanismRootModel {
+ public:
+  RootModel(NT_Inst inst, const wpi::Twine& path, wpi::StringRef name)
+      : m_group{inst, path, name},
+        m_x{nt::GetEntry(inst, path + "/x")},
+        m_y{nt::GetEntry(inst, path + "/y")} {}
+
+  const char* GetName() const final { return m_group.GetName(); }
+  void ForEachObject(
+      wpi::function_ref<void(MechanismObjectModel& model)> func) final {
+    m_group.ForEachObject(func);
+  }
+
+  bool NTUpdate(const nt::EntryNotification& event, wpi::StringRef childName);
+
+  frc::Translation2d GetPosition() const override { return m_pos; };
+
+ private:
+  NTMechanismGroupImpl m_group;
+  NT_Entry m_x;
+  NT_Entry m_y;
+  frc::Translation2d m_pos;
+};
+
+bool NTMechanism2DModel::RootModel::NTUpdate(const nt::EntryNotification& event,
+                                             wpi::StringRef childName) {
+  if ((event.flags & NT_NOTIFY_DELETE) != 0 &&
+      (event.entry == m_x || event.entry == m_y)) {
+    return true;
+  } else if (event.entry == m_x) {
+    if (event.value && event.value->IsDouble()) {
+      m_pos = frc::Translation2d{units::meter_t{event.value->GetDouble()},
+                                 m_pos.Y()};
+    }
+  } else if (event.entry == m_y) {
+    if (event.value && event.value->IsDouble()) {
+      m_pos = frc::Translation2d{m_pos.X(),
+                                 units::meter_t{event.value->GetDouble()}};
+    }
+  } else {
+    m_group.NTUpdate(event, childName);
+  }
+  return false;
+}
+
+NTMechanism2DModel::NTMechanism2DModel(wpi::StringRef path)
+    : NTMechanism2DModel{nt::GetDefaultInstance(), path} {}
+
+NTMechanism2DModel::NTMechanism2DModel(NT_Inst inst, wpi::StringRef path)
+    : m_nt{inst},
+      m_path{(path + "/").str()},
+      m_name{m_nt.GetEntry(path + "/.name")},
+      m_dimensions{m_nt.GetEntry(path + "/dims")},
+      m_bgColor{m_nt.GetEntry(path + "/backgroundColor")},
+      m_dimensionsValue{1_m, 1_m} {
+  m_nt.AddListener(m_path, NT_NOTIFY_LOCAL | NT_NOTIFY_NEW | NT_NOTIFY_DELETE |
+                               NT_NOTIFY_UPDATE | NT_NOTIFY_IMMEDIATE);
+}
+
+NTMechanism2DModel::~NTMechanism2DModel() = default;
+
+void NTMechanism2DModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    // .name
+    if (event.entry == m_name) {
+      if (event.value && event.value->IsString()) {
+        m_nameValue = event.value->GetString();
+      }
+      continue;
+    }
+
+    // dims
+    if (event.entry == m_dimensions) {
+      if (event.value && event.value->IsDoubleArray()) {
+        auto arr = event.value->GetDoubleArray();
+        if (arr.size() == 2) {
+          m_dimensionsValue = frc::Translation2d{units::meter_t{arr[0]},
+                                                 units::meter_t{arr[1]}};
+        }
+      }
+    }
+
+    // backgroundColor
+    if (event.entry == m_bgColor) {
+      if (event.value && event.value->IsString()) {
+        ConvertColor(event.value->GetString(), &m_bgColorValue);
+      }
+    }
+
+    if (wpi::StringRef{event.name}.startswith(m_path)) {
+      auto name = wpi::StringRef{event.name}.drop_front(m_path.size());
+      if (name.empty() || name[0] == '.') {
+        continue;
+      }
+      wpi::StringRef childName;
+      std::tie(name, childName) = name.split('/');
+      if (childName.empty()) {
+        continue;
+      }
+
+      auto it = std::lower_bound(m_roots.begin(), m_roots.end(), name,
+                                 [](const auto& e, wpi::StringRef name) {
+                                   return e->GetName() < name;
+                                 });
+      bool match = it != m_roots.end() && (*it)->GetName() == name;
+
+      if (event.flags & NT_NOTIFY_NEW) {
+        if (!match) {
+          it =
+              m_roots.emplace(it, std::make_unique<RootModel>(
+                                      m_nt.GetInstance(), m_path + name, name));
+          match = true;
+        }
+      }
+      if (match) {
+        if ((*it)->NTUpdate(event, childName)) {
+          m_roots.erase(it);
+        }
+      }
+    }
+  }
+}
+
+bool NTMechanism2DModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_name) != NT_UNASSIGNED;
+}
+
+bool NTMechanism2DModel::IsReadOnly() {
+  return false;
+}
+
+void NTMechanism2DModel::ForEachRoot(
+    wpi::function_ref<void(MechanismRootModel& model)> func) {
+  for (auto&& obj : m_roots) {
+    func(*obj);
+  }
+}

glass/src/libnt/native/cpp/StandardNetworkTables.cpp

@@ -11,6 +11,7 @@
 #include "glass/networktables/NTField2D.h"
 #include "glass/networktables/NTGyro.h"
 #include "glass/networktables/NTMecanumDrive.h"
+#include "glass/networktables/NTMechanism2D.h"
 #include "glass/networktables/NTPIDController.h"
 #include "glass/networktables/NTSpeedController.h"
 #include "glass/networktables/NTStringChooser.h"
@@ -117,6 +118,18 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) {
         return MakeFunctionView(
             [=] { DisplayDrive(static_cast<NTMecanumDriveModel*>(model)); });
       });
+  provider.Register(
+      NTMechanism2DModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTMechanism2DModel>(inst, path);
+      },
+      [=](Window* win, Model* model, const char* path) {
+        win->SetDefaultPos(400, 400);
+        win->SetDefaultSize(200, 200);
+        win->SetPadding(0, 0);
+        return std::make_unique<Mechanism2DView>(
+            static_cast<NTMechanism2DModel*>(model));
+      });
   provider.Register(
       NTPIDControllerModel::kType,
       [](NT_Inst inst, const char* path) {

glass/src/libnt/native/include/glass/networktables/NTMechanism2D.h

@@ -0,0 +1,60 @@
+// 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 <string>
+#include <utility>
+#include <vector>
+
+#include <frc/geometry/Translation2d.h>
+#include <ntcore_cpp.h>
+#include <wpi/StringRef.h>
+
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/other/Mechanism2D.h"
+
+namespace glass {
+
+class NTMechanism2DModel : public Mechanism2DModel {
+ public:
+  static constexpr const char* kType = "Mechanism2d";
+
+  // path is to the table containing ".type", excluding the trailing /
+  explicit NTMechanism2DModel(wpi::StringRef path);
+  NTMechanism2DModel(NT_Inst inst, wpi::StringRef path);
+  ~NTMechanism2DModel() override;
+
+  const char* GetPath() const { return m_path.c_str(); }
+  const char* GetName() const { return m_nameValue.c_str(); }
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override;
+
+  frc::Translation2d GetDimensions() const override {
+    return m_dimensionsValue;
+  }
+  ImU32 GetBackgroundColor() const override { return m_bgColorValue; }
+  void ForEachRoot(
+      wpi::function_ref<void(MechanismRootModel& model)> func) override;
+
+ private:
+  NetworkTablesHelper m_nt;
+  std::string m_path;
+
+  NT_Entry m_name;
+  NT_Entry m_dimensions;
+  NT_Entry m_bgColor;
+
+  std::string m_nameValue;
+  frc::Translation2d m_dimensionsValue;
+  ImU32 m_bgColorValue = 0;
+
+  class RootModel;
+  std::vector<std::unique_ptr<RootModel>> m_roots;
+};
+
+}  // namespace glass

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

@@ -1,330 +0,0 @@
-// 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 "Mechanism2D.h"
-
-#include <cmath>
-#include <string>
-
-#include <hal/SimDevice.h>
-#include <hal/simulation/SimDeviceData.h>
-#include <imgui.h>
-
-#define IMGUI_DEFINE_MATH_OPERATORS
-#include <imgui_internal.h>
-#include <portable-file-dialogs.h>
-#include <wpi/json.h>
-#include <wpi/math>
-#include <wpi/raw_istream.h>
-#include <wpi/raw_ostream.h>
-
-#include "HALSimGui.h"
-
-using namespace halsimgui;
-
-static HAL_SimDeviceHandle devHandle = 0;
-static wpi::StringMap<ImColor> colorLookUpTable;
-static std::unique_ptr<pfd::open_file> m_fileOpener;
-static std::string previousJsonLocation = "Not empty";
-namespace {
-struct BodyConfig {
-  std::string name;
-  std::string type = "line";
-  int length = 100;
-  std::string color = "green";
-  int angle = 0;
-  std::vector<BodyConfig> children;
-  int lineWidth = 1;
-};
-}  // namespace
-static std::vector<BodyConfig> bodyConfigVector;
-namespace {
-struct DrawLineStruct {
-  float xEnd;
-  float yEnd;
-  float angle;
-};
-}  // namespace
-static struct NamedColor {
-  const char* name;
-  ImColor value;
-} staticColors[] = {{"white", IM_COL32(255, 255, 255, 255)},
-                    {"silver", IM_COL32(192, 192, 192, 255)},
-                    {"gray", IM_COL32(128, 128, 128, 255)},
-                    {"black", IM_COL32(0, 0, 0, 255)},
-                    {"red", IM_COL32(255, 0, 0, 255)},
-                    {"maroon", IM_COL32(128, 0, 0, 255)},
-                    {"yellow", IM_COL32(255, 255, 0, 255)},
-                    {"olive", IM_COL32(128, 128, 0, 255)},
-                    {"lime", IM_COL32(0, 255, 0, 255)},
-                    {"green", IM_COL32(0, 128, 0, 255)},
-                    {"aqua", IM_COL32(0, 255, 255, 255)},
-                    {"teal", IM_COL32(0, 128, 128, 255)},
-                    {"blue", IM_COL32(0, 0, 255, 255)},
-                    {"navy", IM_COL32(0, 0, 128, 255)},
-                    {"fuchsia", IM_COL32(255, 0, 255, 255)},
-                    {"purple", IM_COL32(128, 0, 128, 255)}};
-
-static void buildColorTable() {
-  for (auto&& namedColor : staticColors) {
-    colorLookUpTable.try_emplace(namedColor.name, namedColor.value);
-  }
-}
-namespace {
-class Mechanism2DInfo {
- public:
-  std::string jsonLocation;
-};
-}  // namespace
-
-static Mechanism2DInfo mechanism2DInfo;
-
-bool ReadIni(wpi::StringRef name, wpi::StringRef value) {
-  if (name == "jsonLocation") {
-    mechanism2DInfo.jsonLocation = value;
-  } else {
-    return false;
-  }
-  return true;
-}
-
-void WriteIni(ImGuiTextBuffer* out) {
-  out->appendf("[Mechanism2D][Mechanism2D]\njsonLocation=%s\n\n",
-               mechanism2DInfo.jsonLocation.c_str());
-}
-
-// read/write settings to ini file
-static void* Mechanism2DReadOpen(ImGuiContext* ctx,
-                                 ImGuiSettingsHandler* handler,
-                                 const char* name) {
-  if (name == wpi::StringRef{"Mechanism2D"}) {
-    return &mechanism2DInfo;
-  }
-  return nullptr;
-}
-
-static void Mechanism2DReadLine(ImGuiContext* ctx,
-                                ImGuiSettingsHandler* handler, void* entry,
-                                const char* lineStr) {
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
-  if (entry == &mechanism2DInfo) {
-    ReadIni(name, value);
-  }
-}
-
-static void Mechanism2DWriteAll(ImGuiContext* ctx,
-                                ImGuiSettingsHandler* handler,
-                                ImGuiTextBuffer* out_buf) {
-  WriteIni(out_buf);
-}
-
-static void GetJsonFileLocation() {
-  if (m_fileOpener && m_fileOpener->ready(0)) {
-    auto result = m_fileOpener->result();
-    if (!result.empty()) {
-      mechanism2DInfo.jsonLocation = result[0];
-    } else {
-      wpi::errs() << "Can not find json file!!!";
-    }
-  }
-}
-
-DrawLineStruct DrawLine(float startXLocation, float startYLocation, int length,
-                        float angle, ImDrawList* drawList, ImVec2 windowPos,
-                        ImColor color, const BodyConfig& bodyConfig,
-                        const std::string& previousPath) {
-  DrawLineStruct drawLineStruct;
-  drawLineStruct.angle = angle;
-  // Find the current path do the ligament
-  std::string currentPath = previousPath + bodyConfig.name;
-  // Find the angle in radians
-  double radAngle = (drawLineStruct.angle - 90) * wpi::math::pi / 180;
-  // Get the start X and Y location
-  drawLineStruct.xEnd = startXLocation + length * std::cos(radAngle);
-  drawLineStruct.yEnd = startYLocation + length * std::sin(radAngle);
-  // Add the line to the drawList
-  drawList->AddLine(
-      windowPos + ImVec2(startXLocation, startYLocation),
-      windowPos + ImVec2(drawLineStruct.xEnd, drawLineStruct.yEnd), color,
-      bodyConfig.lineWidth);
-  // Return the end X, Y, and angle
-  return drawLineStruct;
-}
-
-static void buildDrawList(float startXLocation, float startYLocation,
-                          ImDrawList* drawList, float previousAngle,
-                          const std::vector<BodyConfig>& subBodyConfigs,
-                          ImVec2 windowPos) {
-  for (BodyConfig const& bodyConfig : subBodyConfigs) {
-    hal::SimDouble angleHandle;
-    hal::SimDouble lengthHandle;
-    float angle = 0;
-    float length = 0;
-    // Get the smallest of width or height
-    double minSize;
-    // Find the min size of the window
-    minSize = ImGui::GetWindowHeight() > ImGui::GetWindowWidth()
-                  ? ImGui::GetWindowWidth()
-                  : ImGui::GetWindowHeight();
-    if (devHandle == 0) {
-      devHandle = HALSIM_GetSimDeviceHandle("Mechanism2D");
-    }
-    // Get the length
-    if (!lengthHandle) {
-      lengthHandle = HALSIM_GetSimValueHandle(
-          devHandle, (bodyConfig.name + "/length").c_str());
-    }
-    if (lengthHandle) {
-      length = lengthHandle.Get();
-    }
-    if (length <= 0) {
-      length = bodyConfig.length;
-    }
-    // Get the angle
-    if (!angleHandle) {
-      angleHandle = HALSIM_GetSimValueHandle(
-          devHandle, (bodyConfig.name + "/angle").c_str());
-    }
-    if (angleHandle) {
-      angle = angleHandle.Get();
-    }
-    // Calculate the next angle to go to
-    float angleToGoTo = angle + bodyConfig.angle + previousAngle;
-    // Draw the first line and get the ending coordinates
-
-    DrawLineStruct drawLine =
-        DrawLine(startXLocation, startYLocation, minSize / 100 * length,
-                 angleToGoTo, drawList, windowPos,
-                 colorLookUpTable[bodyConfig.color], bodyConfig, "");
-
-    // If the line has children then draw them with the stating points being the
-    // end of the parent
-    if (!bodyConfig.children.empty()) {
-      buildDrawList(drawLine.xEnd, drawLine.yEnd, drawList, drawLine.angle,
-                    bodyConfig.children, windowPos);
-    }
-  }
-}
-
-static BodyConfig readSubJson(const std::string& name, wpi::json const& body) {
-  BodyConfig c;
-  try {
-    c.name = name + "/" + body.at("name").get<std::string>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "could not read body name: " << e.what() << '\n';
-  }
-  try {
-    c.length = body.at("length").get<int>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "length '" << c.name
-                << "': could not find length path: " << e.what() << '\n';
-  }
-  try {
-    c.color = body.at("color").get<std::string>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "color '" << c.name
-                << "': could not find color path: " << e.what() << '\n';
-  }
-  try {
-    c.angle = body.at("angle").get<int>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "angle '" << c.name
-                << "': could not find angle path: " << e.what() << '\n';
-  }
-  try {
-    c.lineWidth = body.at("lineWidth").get<int>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "lineWidth '" << c.name
-                << "': could not find lineWidth path: " << e.what() << '\n';
-  }
-  try {
-    for (wpi::json const& child : body.at("children")) {
-      c.children.push_back(readSubJson(c.name, child));
-      wpi::outs() << "Reading Child with name " << c.name << '\n';
-    }
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "could not read body: " << e.what() << '\n';
-  }
-  return c;
-}
-
-static void readJson(std::string jFile) {
-  std::error_code ec;
-  std::string name;
-  wpi::raw_fd_istream is(jFile, ec);
-  if (ec) {
-    wpi::errs() << "could not open '" << jFile << "': " << ec.message() << '\n';
-  }
-  // parse file
-  wpi::json j;
-  try {
-    j = wpi::json::parse(is);
-  } catch (const wpi::json::parse_error& e) {
-    wpi::errs() << "byte " << e.byte << ": " << e.what() << '\n';
-  }
-  // top level must be an object
-  if (!j.is_object()) {
-    wpi::errs() << "must be JSON object\n";
-  }
-  try {
-    name = j.at("name").get<std::string>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "name '" << name
-                << "': could not find name path: " << e.what() << '\n';
-  }
-  try {
-    for (wpi::json const& body : j.at("body")) {
-      bodyConfigVector.push_back(readSubJson(name, body));
-    }
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "could not read body: " << e.what() << '\n';
-  }
-}
-
-static void DisplayAssembly2D() {
-  if (ImGui::BeginPopupContextItem()) {
-    if (ImGui::MenuItem("Load Json")) {
-      m_fileOpener = std::make_unique<pfd::open_file>(
-          "Choose Mechanism2D json", "", std::vector<std::string>{"*.json"});
-    }
-    ImGui::EndPopup();
-  }
-
-  GetJsonFileLocation();
-  if (!mechanism2DInfo.jsonLocation.empty()) {
-    // Only read the json file if it changed
-    if (mechanism2DInfo.jsonLocation != previousJsonLocation) {
-      bodyConfigVector.clear();
-      readJson(mechanism2DInfo.jsonLocation);
-    }
-    previousJsonLocation = mechanism2DInfo.jsonLocation;
-    ImVec2 windowPos = ImGui::GetWindowPos();
-    ImDrawList* drawList = ImGui::GetWindowDrawList();
-    buildDrawList(ImGui::GetWindowWidth() / 2, ImGui::GetWindowHeight(),
-                  drawList, 0, bodyConfigVector, windowPos);
-  }
-}
-
-void Mechanism2D::Initialize() {
-  // hook ini handler to save settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = "Mechanism2D";
-  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
-  iniHandler.ReadOpenFn = Mechanism2DReadOpen;
-  iniHandler.ReadLineFn = Mechanism2DReadLine;
-  iniHandler.WriteAllFn = Mechanism2DWriteAll;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-
-  buildColorTable();
-  if (auto win =
-          HALSimGui::manager.AddWindow("Mechanism 2D", DisplayAssembly2D)) {
-    win->SetVisibility(glass::Window::kHide);
-    win->SetDefaultPos(200, 200);
-    win->SetDefaultSize(600, 600);
-    win->SetPadding(0, 0);
-  }
-}

simulation/halsim_gui/src/main/native/cpp/Mechanism2D.h

@@ -1,14 +0,0 @@
-// 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
-
-namespace halsimgui {
-
-class Mechanism2D {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui

simulation/halsim_gui/src/main/native/cpp/main.cpp

@@ -21,7 +21,6 @@
 #include "DriverStationGui.h"
 #include "EncoderSimGui.h"
 #include "HALSimGui.h"
-#include "Mechanism2D.h"
 #include "NetworkTablesSimGui.h"
 #include "PCMSimGui.h"
 #include "PDPSimGui.h"
@@ -60,7 +59,6 @@ __declspec(dllexport)
   gui::AddInit(AnalogInputSimGui::Initialize);
   gui::AddInit(AnalogOutputSimGui::Initialize);
   gui::AddInit(DIOSimGui::Initialize);
-  gui::AddInit(Mechanism2D::Initialize);
   gui::AddInit(NetworkTablesSimGui::Initialize);
   gui::AddInit(PCMSimGui::Initialize);
   gui::AddInit(PDPSimGui::Initialize);

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

@@ -1,41 +0,0 @@
-// 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/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/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});
+  }
+}

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

@@ -1,42 +0,0 @@
-// 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 <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

wpilibc/src/main/native/include/frc/smartdashboard/Mechanism2d.h

@@ -0,0 +1,81 @@
+// 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 <string>
+
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/StringMap.h>
+#include <wpi/mutex.h>
+
+#include "frc/smartdashboard/MechanismRoot2d.h"
+#include "frc/smartdashboard/Sendable.h"
+#include "frc/smartdashboard/SendableHelper.h"
+#include "frc/util/Color8Bit.h"
+
+namespace frc {
+
+/**
+ * Visual 2D representation of arms, elevators, and general mechanisms; through
+ * a node-based API.
+ *
+ * A Mechanism2d object is published and contains at least one root node. Other
+ * nodes (such as ligaments) are recursively based on other nodes.
+ *
+ * Except for the Mechanism2d container object, none of the objects should be
+ * passed or interacted with by value! Obtain pointers from factory methods such
+ * as Mechanism2d.GetRoot() and MechanismObject2d.Append<>(). The Mechanism2d
+ * container object owns the root nodes, and each node internally owns the nodes
+ * based on it. Beware not to let the Mechanism2d object out of scope - all
+ * nodes will be recursively destructed!
+ *
+ * @see MechanismObject2d
+ * @see MechanismLigament2d
+ * @see MechanismRoot2d
+ */
+class Mechanism2d : public Sendable, public SendableHelper<Mechanism2d> {
+ public:
+  /**
+   * Create a new Mechanism2d with the given dimensions and background color.
+   *
+   * @param width the width
+   * @param height the height
+   */
+  Mechanism2d(double width, double height,
+              const Color8Bit& backgroundColor = {0, 0, 32});
+
+  /**
+   * Get or create a root in this Mechanism2d with the given name and
+   * position.
+   *
+   * <p>If a root with the given name already exists, the given x and y
+   * coordinates are not used.
+   *
+   * @param name the root name
+   * @param x the root x coordinate
+   * @param y the root y coordinate
+   * @return a new root object, or the existing one with the given name.
+   */
+  MechanismRoot2d* GetRoot(wpi::StringRef name, double x, double y);
+
+  /**
+   * Set the Mechanism2d background color.
+   *
+   * @param color the new background color
+   */
+  void SetBackgroundColor(const Color8Bit& color);
+
+  void InitSendable(SendableBuilder& builder) override;
+
+ private:
+  double m_width;
+  double m_height;
+  char m_color[10];
+  mutable wpi::mutex m_mutex;
+  std::shared_ptr<nt::NetworkTable> m_table;
+  wpi::StringMap<std::unique_ptr<MechanismRoot2d>> m_roots;
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h

@@ -0,0 +1,84 @@
+// 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 <networktables/NetworkTableEntry.h>
+#include <units/angle.h>
+
+#include "frc/smartdashboard/MechanismObject2d.h"
+#include "frc/util/Color8Bit.h"
+
+namespace frc {
+
+/**
+ * Ligament node on a Mechanism2d.
+ *
+ * @see Mechanism2d
+ */
+class MechanismLigament2d : public MechanismObject2d {
+ public:
+  MechanismLigament2d(const wpi::Twine& name, double length,
+                      units::degree_t angle, double lineWidth = 6,
+                      const frc::Color8Bit& color = {235, 137, 52});
+
+  /**
+   * Set the ligament color.
+   *
+   * @param color the color of the line
+   */
+  void SetColor(const frc::Color8Bit& color);
+
+  /**
+   * Set the ligament's length.
+   *
+   * @param length the line length
+   */
+  void SetLength(double length);
+
+  /**
+   * Get the ligament length.
+   *
+   * @return the line length
+   */
+  double GetLength();
+
+  /**
+   * Set the ligament's angle relative to its parent.
+   *
+   * @param degrees the angle
+   */
+  void SetAngle(units::degree_t angle);
+
+  /**
+   * Get the ligament's angle relative to its parent.
+   *
+   * @return the angle
+   */
+  double GetAngle();
+
+  /**
+   * Set the line thickness.
+   *
+   * @param weight the line thickness
+   */
+  void SetLineWeight(double lineWidth);
+
+ protected:
+  void UpdateEntries(std::shared_ptr<NetworkTable> table) override;
+
+ private:
+  void Flush();
+  double m_length;
+  nt::NetworkTableEntry m_lengthEntry;
+  double m_angle;
+  nt::NetworkTableEntry m_angleEntry;
+  double m_weight;
+  nt::NetworkTableEntry m_weightEntry;
+  char m_color[10];
+  nt::NetworkTableEntry m_colorEntry;
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/smartdashboard/MechanismObject2d.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 <memory>
+#include <stdexcept>
+#include <string>
+#include <type_traits>
+#include <utility>
+
+#include <networktables/NetworkTable.h>
+#include <wpi/StringMap.h>
+#include <wpi/Twine.h>
+
+namespace frc {
+
+/**
+ * Common base class for all Mechanism2d node types.
+ *
+ * To append another node, call Append with the type of node and its
+ * construction parameters. None of the node types are designed to be
+ * constructed directly, and are owned by their parent node/container - obtain
+ * pointers from the Append function or similar factory methods.
+ *
+ * @see Mechanism2d.
+ */
+class MechanismObject2d {
+  friend class Mechanism2d;
+
+ protected:
+  explicit MechanismObject2d(const wpi::Twine& name);
+
+  /**
+   * Update all entries with new ones from a new table.
+   *
+   * @param table the new table.
+   */
+  virtual void UpdateEntries(std::shared_ptr<NetworkTable> table) = 0;
+
+  mutable wpi::mutex m_mutex;
+
+ public:
+  virtual ~MechanismObject2d() = default;
+
+  /**
+   * Retrieve the object's name.
+   *
+   * @return the object's name relative to its parent.
+   */
+  const std::string& GetName() const;
+
+  /**
+   * Append a Mechanism object that is based on this one.
+   *
+   * @param name the name of the new object.
+   * @param args constructor arguments of the object type.
+   * @return the constructed and appended object, useful for variable
+   * assignments and call chaining.
+   * @throw if an object with the given name already exists.
+   */
+  template <typename T, typename... Args,
+            typename =
+                std::enable_if_t<std::is_convertible_v<T*, MechanismObject2d*>>>
+  T* Append(wpi::StringRef name, Args&&... args) {
+    std::scoped_lock lock(m_mutex);
+    auto& obj = m_objects[name];
+    if (obj) {
+      throw std::runtime_error(("MechanismObject names must be unique! `" +
+                                name + "` was inserted twice!")
+                                   .str());
+    }
+    obj = std::make_unique<T>(name, std::forward<Args>(args)...);
+    T* ex = static_cast<T*>(obj.get());
+    if (m_table) {
+      ex->Update(m_table->GetSubTable(name));
+    }
+    return ex;
+  }
+
+ private:
+  std::string m_name;
+  wpi::StringMap<std::unique_ptr<MechanismObject2d>> m_objects;
+  std::shared_ptr<NetworkTable> m_table;
+  void Update(std::shared_ptr<NetworkTable> table);
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/smartdashboard/MechanismRoot2d.h

@@ -0,0 +1,49 @@
+// 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 <networktables/NetworkTableEntry.h>
+#include <wpi/Twine.h>
+
+#include "MechanismObject2d.h"
+
+namespace frc {
+
+/**
+ * Root Mechanism2d node.
+ *
+ * Do not create objects of this class directly! Obtain pointers from the
+ * Mechanism2d.GetRoot() factory method.
+ */
+class MechanismRoot2d : private MechanismObject2d {
+  friend class Mechanism2d;
+  struct private_init {};
+
+ public:
+  MechanismRoot2d(const wpi::Twine& name, double x, double y,
+                  const private_init&);
+
+  /**
+   * Set the root's position.
+   *
+   * @param x new x coordinate
+   * @param y new y coordinate
+   */
+  void SetPosition(double x, double y);
+
+  using MechanismObject2d::GetName;
+
+  using MechanismObject2d::Append;
+
+ private:
+  void UpdateEntries(std::shared_ptr<NetworkTable> table) override;
+  inline void Flush();
+  double m_x;
+  double m_y;
+  nt::NetworkTableEntry m_posEntry;
+};
+}  // namespace frc

wpilibcExamples/src/main/cpp/examples/Mechanism2d/cpp/Robot.cpp

@@ -0,0 +1,73 @@
+// 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/AnalogPotentiometer.h>
+#include <frc/Encoder.h>
+#include <frc/Joystick.h>
+#include <frc/PWMSparkMax.h>
+#include <frc/TimedRobot.h>
+#include <frc/simulation/SimHooks.h>
+#include <frc/smartdashboard/Mechanism2d.h>
+#include <frc/smartdashboard/MechanismLigament2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <units/angle.h>
+
+/**
+ * This sample program shows how to use Mechanism2d - a visual representation of
+ * arms, elevators, and other mechanisms on dashboards; driven by a node-based
+ * API.
+ *
+ * <p>Ligaments are based on other ligaments or roots, and roots are contained
+ * in the base Mechanism2d object. Use pointers for nodes, and beware not to let
+ * the container out of scope - the appended nodes will be recursively
+ * destructed!
+ */
+class Robot : public frc::TimedRobot {
+  static constexpr double kMetersPerPulse = 0.01;
+  static constexpr double kElevatorMinimumLength = 0.5;
+
+ public:
+  void RobotInit() override {
+    m_elevatorEncoder.SetDistancePerPulse(kMetersPerPulse);
+
+    // publish to dashboard
+    frc::SmartDashboard::PutData("Mech2d", &m_mech);
+  }
+
+  void RobotPeriodic() override {
+    // update the dashboard mechanism's state
+    m_elevator->SetLength(kElevatorMinimumLength +
+                          m_elevatorEncoder.GetDistance());
+    m_wrist->SetAngle(units::degree_t(m_wristPotentiometer.Get()));
+  }
+
+  void TeleopPeriodic() override {
+    m_elevatorMotor.Set(m_joystick.GetRawAxis(0));
+    m_wristMotor.Set(m_joystick.GetRawAxis(1));
+  }
+
+ private:
+  frc::PWMSparkMax m_elevatorMotor{0};
+  frc::PWMSparkMax m_wristMotor{1};
+  frc::Encoder m_elevatorEncoder{0, 1};
+  frc::AnalogPotentiometer m_wristPotentiometer{1, 90};
+  frc::Joystick m_joystick{0};
+
+  // the main mechanism object
+  frc::Mechanism2d m_mech{3, 3};
+  // the mechanism root node
+  frc::MechanismRoot2d* m_root = m_mech.GetRoot("climber", 3, 3);
+  // MechanismLigament2d objects represent each "section"/"stage" of the
+  // mechanism, and are based off the root node or another ligament object
+  frc::MechanismLigament2d* m_elevator =
+      m_root->Append<frc::MechanismLigament2d>("elevator", 1, 90_deg);
+  frc::MechanismLigament2d* m_wrist =
+      m_elevator->Append<frc::MechanismLigament2d>("wrist", 0.5, 90_deg);
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif

wpilibcExamples/src/main/cpp/examples/examples.json

@@ -51,6 +51,14 @@
     "gradlebase": "cpp",
     "commandversion": 2
   },
+  {
+    "name":"Mechanism2d",
+    "foldername":"Mechanism2d",
+    "gradlebase":"cpp",
+    "description":"An example usage of Mechanism2d to display mechanism states on a dashboard.",
+    "tags":[],
+    "commandversion": 2
+  },
   {
     "name": "Solenoids",
     "description": "Demonstrate controlling a single and double solenoid from Joystick buttons.",

wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Mechanism2D.java

@@ -1,50 +0,0 @@
-// 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.
-
-package edu.wpi.first.wpilibj.simulation;
-
-import edu.wpi.first.hal.SimDevice;
-import edu.wpi.first.hal.SimDevice.Direction;
-import edu.wpi.first.hal.SimDouble;
-import java.util.HashMap;
-import java.util.Map;
-
-public class Mechanism2D {
-  private static final SimDevice m_device = SimDevice.create("Mechanism2D");
-  private static final Map<String, SimDouble> m_createdItems = new HashMap<String, SimDouble>();
-
-  /**
-   * Set/Create the angle of a ligament.
-   *
-   * @param ligamentPath json path to the ligament
-   * @param angle to set the ligament
-   */
-  public void setLigamentAngle(String ligamentPath, float angle) {
-    ligamentPath = ligamentPath + "/angle";
-    if (m_device != null) {
-      if (!m_createdItems.containsKey(ligamentPath)) {
-        m_createdItems.put(
-            ligamentPath, m_device.createDouble(ligamentPath, Direction.kInput, angle));
-      }
-      m_createdItems.get(ligamentPath).set(angle);
-    }
-  }
-
-  /**
-   * Set/Create the length of a ligament.
-   *
-   * @param ligamentPath json path to the ligament
-   * @param length to set the ligament
-   */
-  public void setLigamentLength(String ligamentPath, float length) {
-    ligamentPath = ligamentPath + "/length";
-    if (m_device != null) {
-      if (!m_createdItems.containsKey(ligamentPath)) {
-        m_createdItems.put(
-            ligamentPath, m_device.createDouble(ligamentPath, Direction.kInput, length));
-      }
-      m_createdItems.get(ligamentPath).set(length);
-    }
-  }
-}

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Mechanism2d.java

@@ -0,0 +1,107 @@
+// 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.
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.Map.Entry;
+
+/**
+ * Visual 2D representation of arms, elevators, and general mechanisms; through a node-based API.
+ *
+ * <p>A Mechanism2d object is published and contains at least one root node. Other nodes (such as
+ * ligaments) are recursively based on other nodes.
+ *
+ * @see MechanismObject2d
+ * @see MechanismLigament2d
+ * @see MechanismRoot2d
+ */
+public final class Mechanism2d implements Sendable {
+  private static final String kBackgroundColor = "backgroundColor";
+  private NetworkTable m_table;
+  private final Map<String, MechanismRoot2d> m_roots;
+  private final double[] m_dims = new double[2];
+  private String m_color;
+
+  /**
+   * Create a new Mechanism2d with the given dimensions and default color (dark blue).
+   *
+   * @param width the width
+   * @param height the height
+   */
+  public Mechanism2d(double width, double height) {
+    this(width, height, new Color8Bit(0, 0, 32));
+  }
+
+  /**
+   * Create a new Mechanism2d with the given dimensions.
+   *
+   * @param width the width
+   * @param height the height
+   * @param backgroundColor the background color. Defaults to dark blue.
+   */
+  public Mechanism2d(double width, double height, Color8Bit backgroundColor) {
+    m_roots = new HashMap<>();
+    m_dims[0] = width;
+    m_dims[1] = height;
+    setBackgroundColor(backgroundColor);
+  }
+
+  /**
+   * Get or create a root in this Mechanism2d with the given name and position.
+   *
+   * <p>If a root with the given name already exists, the given x and y coordinates are not used.
+   *
+   * @param name the root name
+   * @param x the root x coordinate
+   * @param y the root y coordinate
+   * @return a new root joint object, or the existing one with the given name.
+   */
+  public synchronized MechanismRoot2d getRoot(String name, double x, double y) {
+    var existing = m_roots.get(name);
+    if (existing != null) {
+      return existing;
+    }
+
+    var root = new MechanismRoot2d(name, x, y);
+    m_roots.put(name, root);
+    if (m_table != null) {
+      root.update(m_table.getSubTable(name));
+    }
+    return root;
+  }
+
+  /**
+   * Set the Mechanism2d background color.
+   *
+   * @param color the new color
+   */
+  public synchronized void setBackgroundColor(Color8Bit color) {
+    this.m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue);
+    if (m_table != null) {
+      m_table.getEntry(kBackgroundColor).setString(m_color);
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Mechanism2d");
+    m_table = builder.getTable();
+    m_table.getEntry("dims").setDoubleArray(m_dims);
+    m_table.getEntry(kBackgroundColor).setString(m_color);
+    synchronized (this) {
+      for (Entry<String, MechanismRoot2d> entry : m_roots.entrySet()) {
+        String name = entry.getKey();
+        MechanismRoot2d root = entry.getValue();
+        synchronized (root) {
+          root.update(m_table.getSubTable(name));
+        }
+      }
+    }
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java

@@ -0,0 +1,140 @@
+// 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.
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+/**
+ * Ligament node on a Mechanism2d.
+ *
+ * @see Mechanism2d
+ */
+public class MechanismLigament2d extends MechanismObject2d {
+  private double m_angle;
+  private NetworkTableEntry m_angleEntry;
+  private String m_color;
+  private NetworkTableEntry m_colorEntry;
+  private double m_length;
+  private NetworkTableEntry m_lengthEntry;
+  private double m_weight;
+  private NetworkTableEntry m_weightEntry;
+
+  /** Create a new ligament. */
+  public MechanismLigament2d(
+      String name, double length, double angle, double lineWidth, Color8Bit color) {
+    super(name);
+    setColor(color);
+    setLength(length);
+    setAngle(angle);
+    setLineWeight(lineWidth);
+  }
+
+  /** Create a new ligament with the default color (dark blue) and thickness (6). */
+  public MechanismLigament2d(String name, double length, double angle) {
+    this(name, length, angle, 10, new Color8Bit(235, 137, 52));
+  }
+
+  /**
+   * Set the ligament's angle relative to its parent.
+   *
+   * @param degrees the angle, in degrees
+   */
+  public synchronized void setAngle(double degrees) {
+    m_angle = degrees;
+    flush();
+  }
+
+  /**
+   * Set the ligament's angle relative to its parent.
+   *
+   * @param angle the angle
+   */
+  public synchronized void setAngle(Rotation2d angle) {
+    setAngle(angle.getDegrees());
+  }
+
+  /**
+   * Get the ligament's angle relative to its parent.
+   *
+   * @return the angle, in degrees
+   */
+  public synchronized double getAngle() {
+    if (m_angleEntry != null) {
+      m_angle = m_angleEntry.getDouble(0.0);
+    }
+    return m_angle;
+  }
+
+  /**
+   * Set the ligament's length.
+   *
+   * @param length the line length
+   */
+  public synchronized void setLength(double length) {
+    m_length = length;
+    flush();
+  }
+
+  /**
+   * Get the ligament length.
+   *
+   * @return the line length
+   */
+  public synchronized double getLength() {
+    if (m_lengthEntry != null) {
+      m_length = m_lengthEntry.getDouble(0.0);
+    }
+    return m_length;
+  }
+
+  /**
+   * Set the ligament color.
+   *
+   * @param color the color of the line
+   */
+  public synchronized void setColor(Color8Bit color) {
+    m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue);
+    flush();
+  }
+
+  /**
+   * Set the line thickness.
+   *
+   * @param weight the line thickness
+   */
+  public synchronized void setLineWeight(double weight) {
+    m_weight = weight;
+    flush();
+  }
+
+  @Override
+  protected void updateEntries(NetworkTable table) {
+    table.getEntry(".type").setString("line");
+    m_angleEntry = table.getEntry("angle");
+    m_lengthEntry = table.getEntry("length");
+    m_colorEntry = table.getEntry("color");
+    m_weightEntry = table.getEntry("weight");
+    flush();
+  }
+
+  /** Flush latest data to NT. */
+  private synchronized void flush() {
+    if (m_angleEntry != null) {
+      m_angleEntry.setDouble(m_angle);
+    }
+    if (m_lengthEntry != null) {
+      m_lengthEntry.setDouble(m_length);
+    }
+    if (m_colorEntry != null) {
+      m_colorEntry.setString(m_color);
+    }
+    if (m_weightEntry != null) {
+      m_weightEntry.setDouble(m_weight);
+    }
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismObject2d.java

@@ -0,0 +1,72 @@
+// 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.
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Common base class for all Mechanism2d node types.
+ *
+ * <p>To append another node, call {@link #append(MechanismObject2d)}. Objects that aren't appended
+ * to a published {@link Mechanism2d} container are nonfunctional.
+ *
+ * @see Mechanism2d
+ */
+public abstract class MechanismObject2d {
+  /** Relative to parent. */
+  private final String m_name;
+
+  private NetworkTable m_table;
+  private final Map<String, MechanismObject2d> m_objects = new HashMap<>(1);
+
+  /**
+   * Create a new Mechanism node object.
+   *
+   * @param name the node's name, must be unique.
+   */
+  protected MechanismObject2d(String name) {
+    m_name = name;
+  }
+
+  /**
+   * Append a Mechanism object that is based on this one.
+   *
+   * @param object the object to add.
+   * @return the object given as a parameter, useful for variable assignments and call chaining.
+   * @throws UnsupportedOperationException if the object's name is already used - object names must
+   *     be unique.
+   */
+  public final synchronized <T extends MechanismObject2d> T append(T object) {
+    if (m_objects.containsKey(object.getName())) {
+      throw new UnsupportedOperationException("Mechanism object names must be unique!");
+    }
+    m_objects.put(object.getName(), object);
+    if (m_table != null) {
+      object.update(m_table.getSubTable(object.getName()));
+    }
+    return object;
+  }
+
+  final void update(NetworkTable table) {
+    m_table = table;
+    updateEntries(m_table);
+    for (MechanismObject2d obj : m_objects.values()) {
+      obj.update(m_table.getSubTable(obj.m_name));
+    }
+  }
+
+  /**
+   * Update all entries with new ones from a new table.
+   *
+   * @param table the new table.
+   */
+  protected abstract void updateEntries(NetworkTable table);
+
+  public final String getName() {
+    return m_name;
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismRoot2d.java

@@ -0,0 +1,94 @@
+// 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.
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Root Mechanism2d node.
+ *
+ * <p>Do not create objects of this class directly! Obtain instances from the {@link
+ * Mechanism2d#getRoot(String, double, double)} factory method.
+ *
+ * <p>Append other nodes by using {@link #append(MechanismObject2d)}.
+ */
+public final class MechanismRoot2d {
+  private final String m_name;
+  private NetworkTable m_table;
+  private final Map<String, MechanismObject2d> m_objects = new HashMap<>(1);
+  private double m_x;
+  private NetworkTableEntry m_xEntry;
+  private double m_y;
+  private NetworkTableEntry m_yEntry;
+
+  /**
+   * Package-private constructor for roots.
+   *
+   * @param name name
+   * @param x x coordinate of root (provide only when constructing a root node)
+   * @param y y coordinate of root (provide only when constructing a root node)
+   */
+  MechanismRoot2d(String name, double x, double y) {
+    m_name = name;
+    m_x = x;
+    m_y = y;
+  }
+
+  /**
+   * Append a Mechanism object that is based on this one.
+   *
+   * @param object the object to add.
+   * @return the object given as a parameter, useful for variable assignments and call chaining.
+   * @throws UnsupportedOperationException if the object's name is already used - object names must
+   *     be unique.
+   */
+  public synchronized <T extends MechanismObject2d> T append(T object) {
+    if (m_objects.containsKey(object.getName())) {
+      throw new UnsupportedOperationException("Mechanism object names must be unique!");
+    }
+    m_objects.put(object.getName(), object);
+    if (m_table != null) {
+      object.update(m_table.getSubTable(object.getName()));
+    }
+    return object;
+  }
+
+  /**
+   * Set the root's position.
+   *
+   * @param x new x coordinate
+   * @param y new y coordinate
+   */
+  public synchronized void setPosition(double x, double y) {
+    m_x = x;
+    m_y = y;
+  }
+
+  void update(NetworkTable table) {
+    m_table = table;
+    m_xEntry = m_table.getEntry("x");
+    m_yEntry = m_table.getEntry("y");
+    flush();
+    for (MechanismObject2d obj : m_objects.values()) {
+      obj.update(m_table.getSubTable(obj.getName()));
+    }
+  }
+
+  public String getName() {
+    return m_name;
+  }
+
+  private void flush() {
+    if (m_xEntry != null) {
+      m_xEntry.setDouble(m_x);
+    }
+    if (m_yEntry != null) {
+      m_yEntry.setDouble(m_y);
+    }
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json

@@ -209,6 +209,14 @@
     "mainclass": "Main",
     "commandversion": 2
   },
+  {"name": "Mechanism2d",
+    "description": "An example usage of Mechanism2d to display mechanism states on a dashboard.",
+    "tags": [],
+    "foldername": "mechanism2d",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
   {
     "name": "Motor Controller",
     "description": "Demonstrate controlling a single motor with a joystick",

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Main.java

@@ -0,0 +1,25 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.mechanism2d;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Robot.java

@@ -0,0 +1,67 @@
+// 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.
+
+package edu.wpi.first.wpilibj.examples.mechanism2d;
+
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PWMSparkMax;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * This sample program shows how to use Mechanism2d - a visual representation of arms, elevators,
+ * and other mechanisms on dashboards; driven by a node-based API.
+ *
+ * <p>Ligaments are based on other ligaments or roots, and roots are contained in the base
+ * Mechanism2d object.
+ */
+public class Robot extends TimedRobot {
+  private static final double kMetersPerPulse = 0.01;
+  private static final double kElevatorMinimumLength = 0.5;
+
+  private final PWMSparkMax m_elevatorMotor = new PWMSparkMax(0);
+  private final PWMSparkMax m_wristMotor = new PWMSparkMax(1);
+  private final AnalogPotentiometer m_wristPot = new AnalogPotentiometer(1, 90);
+  private final Encoder m_elevatorEncoder = new Encoder(0, 1);
+  private final Joystick m_joystick = new Joystick(0);
+
+  private MechanismLigament2d m_elevator;
+  private MechanismLigament2d m_wrist;
+
+  @Override
+  public void robotInit() {
+    m_elevatorEncoder.setDistancePerPulse(kMetersPerPulse);
+
+    // the main mechanism object
+    Mechanism2d mech = new Mechanism2d(200, 200);
+    // the mechanism root node
+    MechanismRoot2d root = mech.getRoot("climber", 80, 100);
+
+    // MechanismLigament2d objects represent each "section"/"stage" of the mechanism, and are based
+    // off the root node or another ligament object
+    m_elevator = root.append(new MechanismLigament2d("elevator", 10, 90));
+    m_wrist = m_elevator.append(new MechanismLigament2d("wrist", 6, 90));
+
+    // post the mechanism to the dashboard
+    SmartDashboard.putData("Mech2d", mech);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    // update the dashboard mechanism's state
+    m_elevator.setLength(kElevatorMinimumLength + m_elevatorEncoder.getDistance());
+    m_wrist.setAngle(m_wristPot.get());
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    m_elevatorMotor.set(m_joystick.getRawAxis(0));
+    m_wristMotor.set(m_joystick.getRawAxis(1));
+  }
+}