Implement more protobuf parts

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

@@ -98,6 +98,15 @@ public synchronized MechanismRoot2d getRoot(String name, double x, double y) {
     return root;
   }
 
+  /**
+   * Get all roots in this Mechanism2d.
+   *
+   * @return a map with all the roots.
+   */
+  public synchronized Map<String, MechanismRoot2d> getAllRoots() {
+    return m_roots;
+  }
+
   /**
    * Set the Mechanism2d background color.
    *
@@ -110,6 +119,14 @@ public synchronized void setBackgroundColor(Color8Bit color) {
     }
   }
 
+  public synchronized double getWidth() {
+    return m_dims[0];
+  }
+
+  public synchronized double getHeight() {
+    return m_dims[1];
+  }
+
   @Override
   public void initSendable(NTSendableBuilder builder) {
     builder.setSmartDashboardType("Mechanism2d");

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

@@ -9,6 +9,7 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.StringEntry;
 import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.wpilibj.smartdashboard.proto.MechanismLigament2dProto;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 
 /**
@@ -228,4 +229,6 @@ protected void updateEntries(NetworkTable table) {
     m_weightEntry = table.getDoubleTopic("weight").getEntry(0.0);
     m_weightEntry.set(m_weight);
   }
+
+  public static final MechanismLigament2dProto proto = new MechanismLigament2dProto();
 }

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

@@ -6,6 +6,7 @@
 
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.smartdashboard.proto.MechanismRoot2dProto;
 
 /**
  * Root Mechanism2d node.
@@ -24,13 +25,13 @@ public final class MechanismRoot2d extends MechanismObject2d {
   private DoublePublisher m_yPub;
 
   /**
-   * Package-private constructor for roots.
+   * Constructor for roots. Do not use this constructor!
    *
    * @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) {
+  public MechanismRoot2d(String name, double x, double y) {
     super(name);
     m_x = x;
     m_y = y;
@@ -59,6 +60,24 @@ public synchronized void setPosition(double x, double y) {
     flush();
   }
 
+  /**
+   * Get the root's x coordinate.
+   *
+   * @returns the x coordinate
+   */
+  public synchronized double getX() {
+    return m_x;
+  }
+
+  /**
+   * Get the root's y coordinate.
+   *
+   * @returns the y coordinate
+   */
+  public synchronized double getY() {
+    return m_y;
+  }
+
   @Override
   protected synchronized void updateEntries(NetworkTable table) {
     if (m_xPub != null) {
@@ -80,4 +99,5 @@ private void flush() {
       m_yPub.set(m_y);
     }
   }
+  public static final MechanismRoot2dProto proto = new MechanismRoot2dProto();
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/proto/Mechanism2dProto.java

@@ -0,0 +1,58 @@
+// 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.proto;
+
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.wpilibj.proto.Mechanism2D.ProtobufMechanism2d;
+import edu.wpi.first.wpilibj.proto.Mechanism2D.ProtobufMechanismLigament2d;
+import edu.wpi.first.wpilibj.proto.Mechanism2D.ProtobufMechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismObject2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class Mechanism2dProto implements Protobuf<Mechanism2d, ProtobufMechanism2d> {
+  @Override
+  public Class<Mechanism2d> getTypeClass() {
+    return MechanismRoot2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufMechanism2d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufMechanism2d createMessage() {
+    return ProtobufMechanism2d.newInstance();
+  }
+
+  @Override
+  public Mechanism2d unpack(ProtobufMechanism2d msg) {
+    var mechanism = new Mechanism2d(msg.getX(), msg.getY(), new Color8Bit(msg.getColor()));
+    // Unpack each root into the mechanism
+    for (ProtobufMechanismRoot2d root : msg.getRoots()) {
+      var mechanismRoot = mechanism.getRoot(root.getName(), root.getX(), root.getY());
+      // Unpack the ligaments for each root
+      for (ProtobufMechanismLigament2d ligament : root.getLigaments()) {
+        mechanismRoot.append(MechanismLigament2d.proto.unpack(ligament));
+      }
+    }
+    return mechanism;
+  }
+
+  @Override
+  public void pack(ProtobufMechanism2d msg, Mechanism2d value) {
+    msg.setHeight(value.getHeight());
+    msg.setWidth(value.getWidth());
+    for (MechanismRoot2d root : value.getAllRoots().values()) {
+      var nestedRoot = MechanismRoot2d.proto.createMessage();
+      MechanismRoot2d.proto.pack(nestedRoot, root);
+      msg.addRoots(nestedRoot);
+    }
+  }
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/proto/MechanismLigament2dProto.java

@@ -28,9 +28,9 @@ public ProtobufMechanismLigament2d createMessage() {
 
   @Override
   public MechanismLigament2d unpack(ProtobufMechanismLigament2d msg) {
-    var root = new MechanismLigament2d("", msg.getLength(), msg.getAngle());
-    for (ProtobufMechanismLigament2d l : msg.getLigaments()) {
-      root.append(unpack(l));
+    var root = new MechanismLigament2d(msg.getName(), msg.getLength(), msg.getAngle());
+    for (ProtobufMechanismLigament2d ligament : msg.getLigaments()) {
+      root.append(unpack(ligament));
     }
     return root;
   }
@@ -40,6 +40,7 @@ public void pack(ProtobufMechanismLigament2d msg, MechanismLigament2d value) {
     msg.setAngle(value.getAngle());
     msg.setColor(value.getColor().toHexString());
     msg.setLength(value.getLength());
+    msg.setName(value.getName());
     msg.setWeight(value.getLineWeight());
     for (MechanismObject2d ligament : value.getObjects().values()) {
       var nestedLigament = createMessage();

wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/proto/MechanismRoot2dProto.java

@@ -0,0 +1,50 @@
+// 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.proto;
+
+import edu.wpi.first.util.protobuf.Protobuf;
+import edu.wpi.first.wpilibj.proto.Mechanism2D.ProtobufMechanismLigament2d;
+import edu.wpi.first.wpilibj.proto.Mechanism2D.ProtobufMechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismObject2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class MechanismRoot2dProto implements Protobuf<MechanismRoot2d, ProtobufMechanismRoot2d> {
+  @Override
+  public Class<MechanismRoot2d> getTypeClass() {
+    return MechanismRoot2d.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufMechanismRoot2d.getDescriptor();
+  }
+
+  @Override
+  public ProtobufMechanismRoot2d createMessage() {
+    return ProtobufMechanismRoot2d.newInstance();
+  }
+
+  @Override
+  public MechanismRoot2d unpack(ProtobufMechanismRoot2d msg) {
+    var root = new MechanismRoot2d(msg.getName(), msg.getX(), msg.getY());
+    for (ProtobufMechanismLigament2d ligament : msg.getLigaments()) {
+      root.append(MechanismLigament2d.proto.unpack(ligament));
+    }
+    return root;
+  }
+
+  @Override
+  public void pack(ProtobufMechanismRoot2d msg, MechanismRoot2d value) {
+    msg.setX(value.getX());
+    msg.setY(value.getY());
+    for (MechanismObject2d ligament : value.getObjects().values()) {
+      var nestedLigament = MechanismLigament2d.proto.createMessage();
+      MechanismLigament2d.proto.pack(nestedLigament, (MechanismLigament2d)ligament);
+      msg.addLigaments(nestedLigament);
+    }
+  }
+}

wpilibj/src/main/proto/mechanism2d.proto

@@ -21,8 +21,8 @@ message ProtobufMechanismLigament2d {
 }
 
 message ProtobufMechanism2d {
-    double x = 1;
-    double y = 2;
+    double width = 1;
+    double height = 2;
     string color = 3;
     repeated ProtobufMechanismRoot2d roots = 4;
 }