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