[wpilib] Add Mechanism2d tests and make Java impl match C++ (wpilibsu…

…ite#5527)

wpilibc/src/test/native/cpp/smartdashboard/Mechanism2dTest.cpp

@@ -0,0 +1,85 @@
+// 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 <frc/smartdashboard/MechanismLigament2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc/util/Color8Bit.h>
+
+#include <gtest/gtest.h>
+#include <networktables/NetworkTableInstance.h>
+#include <units/angle.h>
+
+class Mechanism2dTest;
+
+TEST(Mechanism2dTest, Canvas) {
+  frc::Mechanism2d mechanism{5, 10};
+  auto dimsEntry = nt::NetworkTableInstance::GetDefault().GetEntry(
+      "/SmartDashboard/mechanism/dims");
+  auto colorEntry = nt::NetworkTableInstance::GetDefault().GetEntry(
+      "/SmartDashboard/mechanism/backgroundColor");
+  frc::SmartDashboard::PutData("mechanism", &mechanism);
+  frc::SmartDashboard::UpdateValues();
+  EXPECT_EQ(5.0, dimsEntry.GetDoubleArray({})[0]);
+  EXPECT_EQ(10.0, dimsEntry.GetDoubleArray({})[1]);
+  EXPECT_EQ("#000020", colorEntry.GetString(""));
+  mechanism.SetBackgroundColor({255, 255, 255});
+  frc::SmartDashboard::UpdateValues();
+  EXPECT_EQ("#FFFFFF", colorEntry.GetString(""));
+}
+
+TEST(Mechanism2dTest, Root) {
+  frc::Mechanism2d mechanism{5, 10};
+  auto xEntry = nt::NetworkTableInstance::GetDefault().GetEntry(
+      "/SmartDashboard/mechanism/root/x");
+  auto yEntry = nt::NetworkTableInstance::GetDefault().GetEntry(
+      "/SmartDashboard/mechanism/root/y");
+  frc::MechanismRoot2d* root = mechanism.GetRoot("root", 1, 2);
+  frc::SmartDashboard::PutData("mechanism", &mechanism);
+  frc::SmartDashboard::UpdateValues();
+  EXPECT_EQ(1.0, xEntry.GetDouble(0.0));
+  EXPECT_EQ(2.0, yEntry.GetDouble(0.0));
+  root->SetPosition(2, 4);
+  frc::SmartDashboard::UpdateValues();
+  EXPECT_EQ(2.0, xEntry.GetDouble(0.0));
+  EXPECT_EQ(4.0, yEntry.GetDouble(0.0));
+}
+
+TEST(Mechanism2dTest, Ligament) {
+  frc::Mechanism2d mechanism{5, 10};
+  auto angleEntry = nt::NetworkTableInstance::GetDefault().GetEntry(
+      "/SmartDashboard/mechanism/root/ligament/angle");
+  auto colorEntry = nt::NetworkTableInstance::GetDefault().GetEntry(
+      "/SmartDashboard/mechanism/root/ligament/color");
+  auto lengthEntry = nt::NetworkTableInstance::GetDefault().GetEntry(
+      "/SmartDashboard/mechanism/root/ligament/length");
+  auto weightEntry = nt::NetworkTableInstance::GetDefault().GetEntry(
+      "/SmartDashboard/mechanism/root/ligament/weight");
+  frc::MechanismRoot2d* root = mechanism.GetRoot("root", 1, 2);
+  frc::MechanismLigament2d* ligament = root->Append<frc::MechanismLigament2d>(
+      "ligament", 3, units::degree_t{90}, 1, frc::Color8Bit{255, 255, 255});
+  frc::SmartDashboard::PutData("mechanism", &mechanism);
+  EXPECT_EQ(ligament->GetAngle(), angleEntry.GetDouble(0.0));
+  EXPECT_EQ(ligament->GetColor().HexString(), colorEntry.GetString(""));
+  EXPECT_EQ(ligament->GetLength(), lengthEntry.GetDouble(0.0));
+  EXPECT_EQ(ligament->GetLineWeight(), weightEntry.GetDouble(0.0));
+  ligament->SetAngle(units::degree_t{45});
+  ligament->SetColor({0, 0, 0});
+  ligament->SetLength(2);
+  ligament->SetLineWeight(4);
+  frc::SmartDashboard::UpdateValues();
+  EXPECT_EQ(ligament->GetAngle(), angleEntry.GetDouble(0.0));
+  EXPECT_EQ(ligament->GetColor().HexString(), colorEntry.GetString(""));
+  EXPECT_EQ(ligament->GetLength(), lengthEntry.GetDouble(0.0));
+  EXPECT_EQ(ligament->GetLineWeight(), weightEntry.GetDouble(0.0));
+  angleEntry.SetDouble(22.5);
+  colorEntry.SetString("#FF00FF");
+  lengthEntry.SetDouble(4.0);
+  weightEntry.SetDouble(6.0);
+  frc::SmartDashboard::UpdateValues();
+  EXPECT_EQ(ligament->GetAngle(), angleEntry.GetDouble(0.0));
+  EXPECT_EQ(ligament->GetColor().HexString(), colorEntry.GetString(""));
+  EXPECT_EQ(ligament->GetLength(), lengthEntry.GetDouble(0.0));
+  EXPECT_EQ(ligament->GetLineWeight(), weightEntry.GetDouble(0.0));
+}

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

@@ -68,7 +68,7 @@ final synchronized void update(NetworkTable table) {
   }
 
   /**
-   * Update all entries with new ones from a new table.
+   * Update this object's entries with new ones from a new table.
    *
    * @param table the new table.
    */

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

@@ -6,8 +6,6 @@
 
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.NetworkTable;
-import java.util.HashMap;
-import java.util.Map;
 
 /**
  * Root Mechanism2d node.
@@ -19,10 +17,7 @@
  *
  * <p>Append other nodes by using {@link #append(MechanismObject2d)}.
  */
-public final class MechanismRoot2d implements AutoCloseable {
-  private final String m_name;
-  private NetworkTable m_table;
-  private final Map<String, MechanismObject2d> m_objects = new HashMap<>(1);
+public final class MechanismRoot2d extends MechanismObject2d {
   private double m_x;
   private DoublePublisher m_xPub;
   private double m_y;
@@ -36,7 +31,7 @@ public final class MechanismRoot2d implements AutoCloseable {
    * @param y y coordinate of root (provide only when constructing a root node)
    */
   MechanismRoot2d(String name, double x, double y) {
-    m_name = name;
+    super(name);
     m_x = x;
     m_y = y;
   }
@@ -49,29 +44,7 @@ public void close() {
     if (m_yPub != null) {
       m_yPub.close();
     }
-    for (MechanismObject2d obj : m_objects.values()) {
-      obj.close();
-    }
-  }
-
-  /**
-   * Append a Mechanism object that is based on this one.
-   *
-   * @param <T> The object type.
-   * @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;
+    super.close();
   }
 
   /**
@@ -86,24 +59,17 @@ public synchronized void setPosition(double x, double y) {
     flush();
   }
 
-  synchronized void update(NetworkTable table) {
-    m_table = table;
+  @Override
+  protected synchronized void updateEntries(NetworkTable table) {
     if (m_xPub != null) {
       m_xPub.close();
     }
-    m_xPub = m_table.getDoubleTopic("x").publish();
+    m_xPub = table.getDoubleTopic("x").publish();
     if (m_yPub != null) {
       m_yPub.close();
     }
-    m_yPub = m_table.getDoubleTopic("y").publish();
+    m_yPub = table.getDoubleTopic("y").publish();
     flush();
-    for (MechanismObject2d obj : m_objects.values()) {
-      obj.update(m_table.getSubTable(obj.getName()));
-    }
-  }
-
-  public String getName() {
-    return m_name;
   }
 
   private void flush() {

wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/Mechanism2dTest.java

@@ -0,0 +1,99 @@
+// 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 static org.junit.jupiter.api.Assertions.assertArrayEquals;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class Mechanism2dTest {
+  private NetworkTableInstance m_inst;
+
+  @BeforeEach
+  void setUp() {
+    m_inst = NetworkTableInstance.create();
+    SmartDashboard.setNetworkTableInstance(m_inst);
+  }
+
+  @Test
+  void testCanvas() {
+    try (var mechanism = new Mechanism2d(5, 10);
+        var dimsEntry = m_inst.getEntry("/SmartDashboard/mechanism/dims");
+        var colorEntry = m_inst.getEntry("/SmartDashboard/mechanism/backgroundColor")) {
+      SmartDashboard.putData("mechanism", mechanism);
+      SmartDashboard.updateValues();
+      assertArrayEquals(new double[] {5, 10}, dimsEntry.getDoubleArray(new double[0]));
+      assertEquals("#000020", colorEntry.getString(""));
+      mechanism.setBackgroundColor(new Color8Bit(255, 255, 255));
+      SmartDashboard.updateValues();
+      assertEquals("#FFFFFF", colorEntry.getString(""));
+    }
+  }
+
+  @Test
+  void testRoot() {
+    try (var mechanism = new Mechanism2d(5, 10);
+        var xEntry = m_inst.getEntry("/SmartDashboard/mechanism/root/x");
+        var yEntry = m_inst.getEntry("/SmartDashboard/mechanism/root/y")) {
+      final var root = mechanism.getRoot("root", 1, 2);
+      SmartDashboard.putData("mechanism", mechanism);
+      SmartDashboard.updateValues();
+      assertEquals(1, xEntry.getDouble(0));
+      assertEquals(2, yEntry.getDouble(0));
+      root.setPosition(2, 4);
+      SmartDashboard.updateValues();
+      assertEquals(2, xEntry.getDouble(0));
+      assertEquals(4, yEntry.getDouble(0));
+    }
+  }
+
+  @Test
+  void testLigament() {
+    try (var mechanism = new Mechanism2d(5, 10);
+        var angleEntry = m_inst.getEntry("/SmartDashboard/mechanism/root/ligament/angle");
+        var colorEntry = m_inst.getEntry("/SmartDashboard/mechanism/root/ligament/color");
+        var lengthEntry = m_inst.getEntry("/SmartDashboard/mechanism/root/ligament/length");
+        var weightEntry = m_inst.getEntry("/SmartDashboard/mechanism/root/ligament/weight")) {
+      var root = mechanism.getRoot("root", 1, 2);
+      var ligament =
+          root.append(new MechanismLigament2d("ligament", 3, 90, 1, new Color8Bit(255, 255, 255)));
+      SmartDashboard.putData("mechanism", mechanism);
+      SmartDashboard.updateValues();
+      assertEquals(ligament.getAngle(), angleEntry.getDouble(0));
+      assertEquals(ligament.getColor().toHexString(), colorEntry.getString(""));
+      assertEquals(ligament.getLength(), lengthEntry.getDouble(0));
+      assertEquals(ligament.getLineWeight(), weightEntry.getDouble(0));
+      ligament.setAngle(45);
+      ligament.setColor(new Color8Bit(0, 0, 0));
+      ligament.setLength(2);
+      ligament.setLineWeight(4);
+      SmartDashboard.updateValues();
+      assertEquals(ligament.getAngle(), angleEntry.getDouble(0));
+      assertEquals(ligament.getColor().toHexString(), colorEntry.getString(""));
+      assertEquals(ligament.getLength(), lengthEntry.getDouble(0));
+      assertEquals(ligament.getLineWeight(), weightEntry.getDouble(0));
+      angleEntry.setDouble(22.5);
+      colorEntry.setString("#FF00FF");
+      lengthEntry.setDouble(4);
+      weightEntry.setDouble(6);
+      SmartDashboard.updateValues();
+      assertEquals(ligament.getAngle(), angleEntry.getDouble(0));
+      assertEquals(ligament.getColor().toHexString(), colorEntry.getString(""));
+      assertEquals(ligament.getLength(), lengthEntry.getDouble(0));
+      assertEquals(ligament.getLineWeight(), weightEntry.getDouble(0));
+    }
+  }
+
+  @AfterEach
+  void tearDown() {
+    m_inst.close();
+    SmartDashboard.setNetworkTableInstance(NetworkTableInstance.getDefault());
+  }
+}