SCRIPT Move java files

wpilibj/src/main/java/org/wpilib/smartdashboard/Field2d.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 static edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * 2D representation of game field for dashboards.
+ *
+ * <p>An object's pose is the location shown on the dashboard view. Note that for the robot, this
+ * may or may not match the internal odometry. For example, the robot is shown at a particular
+ * starting location, the pose in this class would represent the actual location on the field, but
+ * the robot's internal state might have a 0,0,0 pose (unless it's initialized to something
+ * different).
+ *
+ * <p>As the user is able to edit the pose, code performing updates should get the robot pose,
+ * transform it as appropriate (e.g. based on wheel odometry), and set the new pose.
+ *
+ * <p>This class provides methods to set the robot pose, but other objects can also be shown by
+ * using the getObject() function. Other objects can also have multiple poses (which will show the
+ * object at multiple locations).
+ */
+public class Field2d implements NTSendable, AutoCloseable {
+  /** Constructor. */
+  @SuppressWarnings("this-escape")
+  public Field2d() {
+    FieldObject2d obj = new FieldObject2d("Robot");
+    obj.setPose(Pose2d.kZero);
+    m_objects.add(obj);
+    SendableRegistry.add(this, "Field");
+  }
+
+  @Override
+  public void close() {
+    for (FieldObject2d obj : m_objects) {
+      obj.close();
+    }
+  }
+
+  /**
+   * Set the robot pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  public synchronized void setRobotPose(Pose2d pose) {
+    m_objects.get(0).setPose(pose);
+  }
+
+  /**
+   * Set the robot pose from x, y, and rotation.
+   *
+   * @param x X location, in meters
+   * @param y Y location, in meters
+   * @param rotation rotation
+   */
+  public synchronized void setRobotPose(double x, double y, Rotation2d rotation) {
+    m_objects.get(0).setPose(x, y, rotation);
+  }
+
+  /**
+   * Set the robot pose from x, y, and rotation.
+   *
+   * @param x X location
+   * @param y Y location
+   * @param rotation rotation
+   */
+  public synchronized void setRobotPose(Distance x, Distance y, Rotation2d rotation) {
+    m_objects.get(0).setPose(x.in(Meters), y.in(Meters), rotation);
+  }
+
+  /**
+   * Get the robot pose.
+   *
+   * @return 2D pose
+   */
+  public synchronized Pose2d getRobotPose() {
+    return m_objects.get(0).getPose();
+  }
+
+  /**
+   * Get or create a field object.
+   *
+   * @param name The field object's name.
+   * @return Field object
+   */
+  public synchronized FieldObject2d getObject(String name) {
+    for (FieldObject2d obj : m_objects) {
+      if (obj.m_name.equals(name)) {
+        return obj;
+      }
+    }
+    FieldObject2d obj = new FieldObject2d(name);
+    m_objects.add(obj);
+    if (m_table != null) {
+      synchronized (obj) {
+        obj.m_entry = m_table.getDoubleArrayTopic(name).getEntry(new double[] {});
+      }
+    }
+    return obj;
+  }
+
+  /**
+   * Get the robot object.
+   *
+   * @return Field object for robot
+   */
+  public synchronized FieldObject2d getRobotObject() {
+    return m_objects.get(0);
+  }
+
+  @Override
+  public void initSendable(NTSendableBuilder builder) {
+    builder.setSmartDashboardType("Field2d");
+
+    synchronized (this) {
+      m_table = builder.getTable();
+      for (FieldObject2d obj : m_objects) {
+        synchronized (obj) {
+          obj.m_entry = m_table.getDoubleArrayTopic(obj.m_name).getEntry(new double[] {});
+          obj.updateEntry(true);
+        }
+      }
+    }
+  }
+
+  private NetworkTable m_table;
+  private final List<FieldObject2d> m_objects = new ArrayList<>();
+}

wpilibj/src/main/java/org/wpilib/smartdashboard/FieldObject2d.java

@@ -0,0 +1,173 @@
+// 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 edu.wpi.first.units.Units.Meters;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.networktables.DoubleArrayEntry;
+import edu.wpi.first.units.measure.Distance;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+/** Game field object on a Field2d. */
+public class FieldObject2d implements AutoCloseable {
+  /**
+   * Package-local constructor.
+   *
+   * @param name name
+   */
+  FieldObject2d(String name) {
+    m_name = name;
+  }
+
+  @Override
+  public void close() {
+    if (m_entry != null) {
+      m_entry.close();
+    }
+  }
+
+  /**
+   * Set the pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  public synchronized void setPose(Pose2d pose) {
+    setPoses(pose);
+  }
+
+  /**
+   * Set the pose from x, y, and rotation.
+   *
+   * @param x X location, in meters
+   * @param y Y location, in meters
+   * @param rotation rotation
+   */
+  public synchronized void setPose(double x, double y, Rotation2d rotation) {
+    setPose(new Pose2d(x, y, rotation));
+  }
+
+  /**
+   * Set the pose from x, y, and rotation.
+   *
+   * @param x X location
+   * @param y Y location
+   * @param rotation rotation
+   */
+  public synchronized void setPose(Distance x, Distance y, Rotation2d rotation) {
+    setPose(new Pose2d(x.in(Meters), y.in(Meters), rotation));
+  }
+
+  /**
+   * Get the pose.
+   *
+   * @return 2D pose
+   */
+  public synchronized Pose2d getPose() {
+    updateFromEntry();
+    if (m_poses.isEmpty()) {
+      return Pose2d.kZero;
+    }
+    return m_poses.get(0);
+  }
+
+  /**
+   * Set multiple poses from a list of Pose objects. The total number of poses is limited to 85.
+   *
+   * @param poses list of 2D poses
+   */
+  public synchronized void setPoses(List<Pose2d> poses) {
+    m_poses.clear();
+    m_poses.addAll(poses);
+    updateEntry();
+  }
+
+  /**
+   * Set multiple poses from a list of Pose objects. The total number of poses is limited to 85.
+   *
+   * @param poses list of 2D poses
+   */
+  public synchronized void setPoses(Pose2d... poses) {
+    m_poses.clear();
+    Collections.addAll(m_poses, poses);
+    updateEntry();
+  }
+
+  /**
+   * Sets poses from a trajectory.
+   *
+   * @param trajectory The trajectory from which the poses should be added.
+   */
+  public synchronized void setTrajectory(Trajectory trajectory) {
+    m_poses.clear();
+    for (Trajectory.State state : trajectory.getStates()) {
+      m_poses.add(state.pose);
+    }
+    updateEntry();
+  }
+
+  /**
+   * Get multiple poses.
+   *
+   * @return list of 2D poses
+   */
+  public synchronized List<Pose2d> getPoses() {
+    updateFromEntry();
+    return new ArrayList<>(m_poses);
+  }
+
+  void updateEntry() {
+    updateEntry(false);
+  }
+
+  synchronized void updateEntry(boolean setDefault) {
+    if (m_entry == null) {
+      return;
+    }
+
+    double[] arr = new double[m_poses.size() * 3];
+    int ndx = 0;
+    for (Pose2d pose : m_poses) {
+      Translation2d translation = pose.getTranslation();
+      arr[ndx + 0] = translation.getX();
+      arr[ndx + 1] = translation.getY();
+      arr[ndx + 2] = pose.getRotation().getDegrees();
+      ndx += 3;
+    }
+
+    if (setDefault) {
+      m_entry.setDefault(arr);
+    } else {
+      m_entry.set(arr);
+    }
+  }
+
+  private synchronized void updateFromEntry() {
+    if (m_entry == null) {
+      return;
+    }
+
+    double[] arr = m_entry.get(null);
+    if (arr != null) {
+      if ((arr.length % 3) != 0) {
+        return;
+      }
+
+      m_poses.clear();
+      for (int i = 0; i < arr.length; i += 3) {
+        m_poses.add(new Pose2d(arr[i], arr[i + 1], Rotation2d.fromDegrees(arr[i + 2])));
+      }
+    }
+  }
+
+  String m_name;
+  DoubleArrayEntry m_entry;
+  private final List<Pose2d> m_poses = new ArrayList<>();
+}

wpilibj/src/main/java/org/wpilib/smartdashboard/ListenerExecutor.java

@@ -0,0 +1,45 @@
+// 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 java.util.ArrayList;
+import java.util.Collection;
+import java.util.concurrent.Executor;
+
+/**
+ * An executor for running listener tasks posted by {@link edu.wpi.first.wpilibj.Sendable} listeners
+ * synchronously from the main application thread.
+ */
+class ListenerExecutor implements Executor {
+  private final Collection<Runnable> m_tasks = new ArrayList<>();
+  private final Object m_lock = new Object();
+
+  /**
+   * Posts a task to the executor to be run synchronously from the main thread.
+   *
+   * @param task The task to run synchronously from the main thread.
+   */
+  @Override
+  public void execute(Runnable task) {
+    synchronized (m_lock) {
+      m_tasks.add(task);
+    }
+  }
+
+  /** Runs all posted tasks. Called periodically from main thread. */
+  public void runListenerTasks() {
+    // Locally copy tasks from internal list; minimizes blocking time
+    Collection<Runnable> tasks;
+    synchronized (m_lock) {
+      tasks = new ArrayList<>(m_tasks);
+      m_tasks.clear();
+    }
+
+    // Run all tasks
+    for (Runnable task : tasks) {
+      task.run();
+    }
+  }
+}

wpilibj/src/main/java/org/wpilib/smartdashboard/Mechanism2d.java

@@ -0,0 +1,137 @@
+// 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.DoubleArrayPublisher;
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.StringPublisher;
+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. A root is the anchor
+ * point of other nodes (such as ligaments). Other nodes are recursively appended based on other
+ * nodes.
+ *
+ * @see MechanismObject2d
+ * @see MechanismLigament2d
+ * @see MechanismRoot2d
+ */
+public final class Mechanism2d implements NTSendable, AutoCloseable {
+  private NetworkTable m_table;
+  private final Map<String, MechanismRoot2d> m_roots;
+  private final double[] m_dims = new double[2];
+  private String m_color;
+  private DoubleArrayPublisher m_dimsPub;
+  private StringPublisher m_colorPub;
+
+  /**
+   * Create a new Mechanism2d with the given dimensions and default color (dark blue).
+   *
+   * <p>The dimensions represent the canvas that all the nodes are drawn on.
+   *
+   * @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.
+   *
+   * <p>The dimensions represent the canvas that all the nodes are drawn on.
+   *
+   * @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);
+  }
+
+  @Override
+  public void close() {
+    if (m_dimsPub != null) {
+      m_dimsPub.close();
+    }
+    if (m_colorPub != null) {
+      m_colorPub.close();
+    }
+    for (MechanismRoot2d root : m_roots.values()) {
+      root.close();
+    }
+  }
+
+  /**
+   * 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) {
+    m_color = color.toHexString();
+    if (m_colorPub != null) {
+      m_colorPub.set(m_color);
+    }
+  }
+
+  @Override
+  public void initSendable(NTSendableBuilder builder) {
+    builder.setSmartDashboardType("Mechanism2d");
+    synchronized (this) {
+      m_table = builder.getTable();
+      if (m_dimsPub != null) {
+        m_dimsPub.close();
+      }
+      m_dimsPub = m_table.getDoubleArrayTopic("dims").publish();
+      m_dimsPub.set(m_dims);
+      if (m_colorPub != null) {
+        m_colorPub.close();
+      }
+      m_colorPub = m_table.getStringTopic("backgroundColor").publish();
+      m_colorPub.set(m_color);
+      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/org/wpilib/smartdashboard/MechanismLigament2d.java

@@ -0,0 +1,238 @@
+// 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.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.DoubleEntry;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.StringEntry;
+import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.networktables.StringTopic;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+/**
+ * Ligament node on a Mechanism2d. A ligament can have its length changed (like an elevator) or
+ * angle changed, like an arm.
+ *
+ * @see Mechanism2d
+ */
+public class MechanismLigament2d extends MechanismObject2d {
+  private StringPublisher m_typePub;
+  private double m_angle;
+  private DoubleEntry m_angleEntry;
+  private String m_color;
+  private StringEntry m_colorEntry;
+  private double m_length;
+  private DoubleEntry m_lengthEntry;
+  private double m_weight;
+  private DoubleEntry m_weightEntry;
+
+  private static String kSmartDashboardType = "line";
+
+  /**
+   * Create a new ligament.
+   *
+   * @param name The ligament name.
+   * @param length The ligament length.
+   * @param angle The ligament angle in degrees.
+   * @param lineWidth The ligament's line width.
+   * @param color The ligament's color.
+   */
+  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 (orange) and thickness (6).
+   *
+   * @param name The ligament's name.
+   * @param length The ligament's length.
+   * @param angle The ligament's angle relative to its parent in degrees.
+   */
+  public MechanismLigament2d(String name, double length, double angle) {
+    this(name, length, angle, 10, new Color8Bit(235, 137, 52));
+  }
+
+  @Override
+  public void close() {
+    super.close();
+    if (m_typePub != null) {
+      m_typePub.close();
+    }
+    if (m_angleEntry != null) {
+      m_angleEntry.close();
+    }
+    if (m_colorEntry != null) {
+      m_colorEntry.close();
+    }
+    if (m_lengthEntry != null) {
+      m_lengthEntry.close();
+    }
+    if (m_weightEntry != null) {
+      m_weightEntry.close();
+    }
+  }
+
+  /**
+   * Set the ligament's angle relative to its parent.
+   *
+   * @param degrees the angle in degrees
+   */
+  public final synchronized void setAngle(double degrees) {
+    m_angle = degrees;
+    if (m_angleEntry != null) {
+      m_angleEntry.set(degrees);
+    }
+  }
+
+  /**
+   * 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.get();
+    }
+    return m_angle;
+  }
+
+  /**
+   * Set the ligament's length.
+   *
+   * @param length the line length
+   */
+  public final synchronized void setLength(double length) {
+    m_length = length;
+    if (m_lengthEntry != null) {
+      m_lengthEntry.set(length);
+    }
+  }
+
+  /**
+   * Get the ligament length.
+   *
+   * @return the line length
+   */
+  public synchronized double getLength() {
+    if (m_lengthEntry != null) {
+      m_length = m_lengthEntry.get();
+    }
+    return m_length;
+  }
+
+  /**
+   * Set the ligament color.
+   *
+   * @param color the color of the line
+   */
+  public final synchronized void setColor(Color8Bit color) {
+    m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue);
+    if (m_colorEntry != null) {
+      m_colorEntry.set(m_color);
+    }
+  }
+
+  /**
+   * Get the ligament color.
+   *
+   * @return the color of the line
+   */
+  public synchronized Color8Bit getColor() {
+    if (m_colorEntry != null) {
+      m_color = m_colorEntry.get();
+    }
+    int r = 0;
+    int g = 0;
+    int b = 0;
+    if (m_color.length() == 7 && m_color.charAt(0) == '#') {
+      try {
+        r = Integer.parseInt(m_color.substring(1, 3), 16);
+        g = Integer.parseInt(m_color.substring(3, 5), 16);
+        b = Integer.parseInt(m_color.substring(5, 7), 16);
+      } catch (NumberFormatException e) {
+        r = 0;
+        g = 0;
+        b = 0;
+      }
+    }
+    return new Color8Bit(r, g, b);
+  }
+
+  /**
+   * Set the line thickness.
+   *
+   * @param weight the line thickness
+   */
+  public final synchronized void setLineWeight(double weight) {
+    m_weight = weight;
+    if (m_weightEntry != null) {
+      m_weightEntry.set(weight);
+    }
+  }
+
+  /**
+   * Get the line thickness.
+   *
+   * @return the line thickness
+   */
+  public synchronized double getLineWeight() {
+    if (m_weightEntry != null) {
+      m_weight = m_weightEntry.get();
+    }
+    return m_weight;
+  }
+
+  @Override
+  protected void updateEntries(NetworkTable table) {
+    if (m_typePub != null) {
+      m_typePub.close();
+    }
+    m_typePub =
+        table
+            .getStringTopic(".type")
+            .publishEx(
+                StringTopic.kTypeString, "{\"SmartDashboard\":\"" + kSmartDashboardType + "\"}");
+    m_typePub.set(kSmartDashboardType);
+
+    if (m_angleEntry != null) {
+      m_angleEntry.close();
+    }
+    m_angleEntry = table.getDoubleTopic("angle").getEntry(0.0);
+    m_angleEntry.set(m_angle);
+
+    if (m_lengthEntry != null) {
+      m_lengthEntry.close();
+    }
+    m_lengthEntry = table.getDoubleTopic("length").getEntry(0.0);
+    m_lengthEntry.set(m_length);
+
+    if (m_colorEntry != null) {
+      m_colorEntry.close();
+    }
+    m_colorEntry = table.getStringTopic("color").getEntry("");
+    m_colorEntry.set(m_color);
+
+    if (m_weightEntry != null) {
+      m_weightEntry.close();
+    }
+    m_weightEntry = table.getDoubleTopic("weight").getEntry(0.0);
+    m_weightEntry.set(m_weight);
+  }
+}

wpilibj/src/main/java/org/wpilib/smartdashboard/MechanismObject2d.java

@@ -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.
+
+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 implements AutoCloseable {
+  /** 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;
+  }
+
+  @Override
+  public void 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 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 synchronized 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 this object's entries with new ones from a new table.
+   *
+   * @param table the new table.
+   */
+  protected abstract void updateEntries(NetworkTable table);
+
+  /**
+   * Retrieve the object's name.
+   *
+   * @return the object's name relative to its parent.
+   */
+  public final String getName() {
+    return m_name;
+  }
+}

wpilibj/src/main/java/org/wpilib/smartdashboard/MechanismRoot2d.java

@@ -0,0 +1,83 @@
+// 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.DoublePublisher;
+import edu.wpi.first.networktables.NetworkTable;
+
+/**
+ * Root Mechanism2d node.
+ *
+ * <p>A root is the anchor point of other nodes (such as ligaments).
+ *
+ * <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 extends MechanismObject2d {
+  private double m_x;
+  private DoublePublisher m_xPub;
+  private double m_y;
+  private DoublePublisher m_yPub;
+
+  /**
+   * 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) {
+    super(name);
+    m_x = x;
+    m_y = y;
+  }
+
+  @Override
+  public void close() {
+    if (m_xPub != null) {
+      m_xPub.close();
+    }
+    if (m_yPub != null) {
+      m_yPub.close();
+    }
+    super.close();
+  }
+
+  /**
+   * 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;
+    flush();
+  }
+
+  @Override
+  protected synchronized void updateEntries(NetworkTable table) {
+    if (m_xPub != null) {
+      m_xPub.close();
+    }
+    m_xPub = table.getDoubleTopic("x").publish();
+    if (m_yPub != null) {
+      m_yPub.close();
+    }
+    m_yPub = table.getDoubleTopic("y").publish();
+    flush();
+  }
+
+  private void flush() {
+    if (m_xPub != null) {
+      m_xPub.set(m_x);
+    }
+    if (m_yPub != null) {
+      m_yPub.set(m_y);
+    }
+  }
+}

wpilibj/src/main/java/org/wpilib/smartdashboard/SendableBuilderImpl.java

@@ -0,0 +1,685 @@
+// 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.BooleanArrayPublisher;
+import edu.wpi.first.networktables.BooleanArraySubscriber;
+import edu.wpi.first.networktables.BooleanArrayTopic;
+import edu.wpi.first.networktables.BooleanPublisher;
+import edu.wpi.first.networktables.BooleanSubscriber;
+import edu.wpi.first.networktables.BooleanTopic;
+import edu.wpi.first.networktables.DoubleArrayPublisher;
+import edu.wpi.first.networktables.DoubleArraySubscriber;
+import edu.wpi.first.networktables.DoubleArrayTopic;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.DoubleSubscriber;
+import edu.wpi.first.networktables.DoubleTopic;
+import edu.wpi.first.networktables.FloatArrayPublisher;
+import edu.wpi.first.networktables.FloatArraySubscriber;
+import edu.wpi.first.networktables.FloatArrayTopic;
+import edu.wpi.first.networktables.FloatPublisher;
+import edu.wpi.first.networktables.FloatSubscriber;
+import edu.wpi.first.networktables.FloatTopic;
+import edu.wpi.first.networktables.IntegerArrayPublisher;
+import edu.wpi.first.networktables.IntegerArraySubscriber;
+import edu.wpi.first.networktables.IntegerArrayTopic;
+import edu.wpi.first.networktables.IntegerPublisher;
+import edu.wpi.first.networktables.IntegerSubscriber;
+import edu.wpi.first.networktables.IntegerTopic;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.PubSubOption;
+import edu.wpi.first.networktables.Publisher;
+import edu.wpi.first.networktables.RawPublisher;
+import edu.wpi.first.networktables.RawSubscriber;
+import edu.wpi.first.networktables.RawTopic;
+import edu.wpi.first.networktables.StringArrayPublisher;
+import edu.wpi.first.networktables.StringArraySubscriber;
+import edu.wpi.first.networktables.StringArrayTopic;
+import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.networktables.StringSubscriber;
+import edu.wpi.first.networktables.StringTopic;
+import edu.wpi.first.networktables.Subscriber;
+import edu.wpi.first.networktables.Topic;
+import edu.wpi.first.util.function.BooleanConsumer;
+import edu.wpi.first.util.function.FloatConsumer;
+import edu.wpi.first.util.function.FloatSupplier;
+import edu.wpi.first.wpilibj.RobotController;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+import java.util.function.DoubleConsumer;
+import java.util.function.DoubleSupplier;
+import java.util.function.LongConsumer;
+import java.util.function.LongSupplier;
+import java.util.function.Supplier;
+
+/** Implementation detail for SendableBuilder. */
+public class SendableBuilderImpl implements NTSendableBuilder {
+  @FunctionalInterface
+  private interface TimedConsumer<T> {
+    void accept(T value, long time);
+  }
+
+  private static final class Property<P extends Publisher, S extends Subscriber>
+      implements AutoCloseable {
+    @Override
+    @SuppressWarnings("PMD.AvoidCatchingGenericException")
+    public void close() {
+      try {
+        if (m_pub != null) {
+          m_pub.close();
+        }
+        if (m_sub != null) {
+          m_sub.close();
+        }
+      } catch (Exception e) {
+        // ignore
+      }
+    }
+
+    void update(boolean controllable, long time) {
+      if (controllable && m_sub != null && m_updateLocal != null) {
+        m_updateLocal.accept(m_sub);
+      }
+      if (m_pub != null && m_updateNetwork != null) {
+        m_updateNetwork.accept(m_pub, time);
+      }
+    }
+
+    P m_pub;
+    S m_sub;
+    TimedConsumer<P> m_updateNetwork;
+    Consumer<S> m_updateLocal;
+  }
+
+  private final List<Property<?, ?>> m_properties = new ArrayList<>();
+  private final List<Runnable> m_updateTables = new ArrayList<>();
+  private NetworkTable m_table;
+  private boolean m_controllable;
+  private boolean m_actuator;
+
+  private BooleanPublisher m_controllablePub;
+  private StringPublisher m_typePub;
+  private BooleanPublisher m_actuatorPub;
+
+  private final List<AutoCloseable> m_closeables = new ArrayList<>();
+
+  /** Default constructor. */
+  public SendableBuilderImpl() {}
+
+  @Override
+  @SuppressWarnings("PMD.AvoidCatchingGenericException")
+  public void close() {
+    if (m_controllablePub != null) {
+      m_controllablePub.close();
+    }
+    if (m_typePub != null) {
+      m_typePub.close();
+    }
+    if (m_actuatorPub != null) {
+      m_actuatorPub.close();
+    }
+    for (Property<?, ?> property : m_properties) {
+      property.close();
+    }
+    for (AutoCloseable closeable : m_closeables) {
+      try {
+        closeable.close();
+      } catch (Exception e) {
+        // ignore
+      }
+    }
+  }
+
+  /**
+   * Set the network table. Must be called prior to any Add* functions being called.
+   *
+   * @param table Network table
+   */
+  public void setTable(NetworkTable table) {
+    m_table = table;
+    m_controllablePub = table.getBooleanTopic(".controllable").publish();
+    m_controllablePub.setDefault(false);
+  }
+
+  /**
+   * Get the network table.
+   *
+   * @return The network table
+   */
+  @Override
+  public NetworkTable getTable() {
+    return m_table;
+  }
+
+  /**
+   * Return whether this sendable has an associated table.
+   *
+   * @return True if it has a table, false if not.
+   */
+  @Override
+  public boolean isPublished() {
+    return m_table != null;
+  }
+
+  /**
+   * Return whether this sendable should be treated as an actuator.
+   *
+   * @return True if actuator, false if not.
+   */
+  public boolean isActuator() {
+    return m_actuator;
+  }
+
+  /** Update the network table values by calling the getters for all properties. */
+  @Override
+  public void update() {
+    long time = RobotController.getTime();
+    for (Property<?, ?> property : m_properties) {
+      property.update(m_controllable, time);
+    }
+    for (Runnable updateTable : m_updateTables) {
+      updateTable.run();
+    }
+  }
+
+  /** Hook setters for all properties. */
+  public void startListeners() {
+    m_controllable = true;
+    if (m_controllablePub != null) {
+      m_controllablePub.set(true);
+    }
+  }
+
+  /** Unhook setters for all properties. */
+  public void stopListeners() {
+    m_controllable = false;
+    if (m_controllablePub != null) {
+      m_controllablePub.set(false);
+    }
+  }
+
+  /** Clear properties. */
+  @Override
+  public void clearProperties() {
+    stopListeners();
+    for (Property<?, ?> property : m_properties) {
+      property.close();
+    }
+    m_properties.clear();
+  }
+
+  @Override
+  public void addCloseable(AutoCloseable closeable) {
+    m_closeables.add(closeable);
+  }
+
+  /**
+   * Set the string representation of the named data type that will be used by the smart dashboard
+   * for this sendable.
+   *
+   * @param type data type
+   */
+  @Override
+  public void setSmartDashboardType(String type) {
+    if (m_typePub == null) {
+      m_typePub =
+          m_table
+              .getStringTopic(".type")
+              .publishEx(StringTopic.kTypeString, "{\"SmartDashboard\":\"" + type + "\"}");
+    }
+    m_typePub.set(type);
+  }
+
+  /**
+   * Set a flag indicating if this sendable should be treated as an actuator. By default, this flag
+   * is false.
+   *
+   * @param value true if actuator, false if not
+   */
+  @Override
+  public void setActuator(boolean value) {
+    if (m_actuatorPub == null) {
+      m_actuatorPub = m_table.getBooleanTopic(".actuator").publish();
+    }
+    m_actuatorPub.set(value);
+    m_actuator = value;
+  }
+
+  /**
+   * Set the function that should be called to update the network table for things other than
+   * properties. Note this function is not passed the network table object; instead it should use
+   * the topics returned by getTopic().
+   *
+   * @param func function
+   */
+  @Override
+  public void setUpdateTable(Runnable func) {
+    m_updateTables.add(func);
+  }
+
+  /**
+   * Add a property without getters or setters. This can be used to get entry handles for the
+   * function called by setUpdateTable().
+   *
+   * @param key property name
+   * @return Network table entry
+   */
+  @Override
+  public Topic getTopic(String key) {
+    return m_table.getTopic(key);
+  }
+
+  /**
+   * Add a boolean property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  @Override
+  public void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter) {
+    Property<BooleanPublisher, BooleanSubscriber> property = new Property<>();
+    BooleanTopic topic = m_table.getBooleanTopic(key);
+    if (getter != null) {
+      property.m_pub = topic.publish();
+      property.m_updateNetwork = (pub, time) -> pub.set(getter.getAsBoolean(), time);
+    }
+    if (setter != null) {
+      property.m_sub = topic.subscribe(false, PubSubOption.excludePublisher(property.m_pub));
+      property.m_updateLocal =
+          sub -> {
+            for (boolean val : sub.readQueueValues()) {
+              setter.accept(val);
+            }
+          };
+    }
+    m_properties.add(property);
+  }
+
+  @Override
+  public void publishConstBoolean(String key, boolean value) {
+    Property<BooleanPublisher, BooleanSubscriber> property = new Property<>();
+    BooleanTopic topic = m_table.getBooleanTopic(key);
+    property.m_pub = topic.publish();
+    property.m_pub.set(value);
+    m_properties.add(property);
+  }
+
+  /**
+   * Add an integer property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  @Override
+  public void addIntegerProperty(String key, LongSupplier getter, LongConsumer setter) {
+    Property<IntegerPublisher, IntegerSubscriber> property = new Property<>();
+    IntegerTopic topic = m_table.getIntegerTopic(key);
+    if (getter != null) {
+      property.m_pub = topic.publish();
+      property.m_updateNetwork = (pub, time) -> pub.set(getter.getAsLong(), time);
+    }
+    if (setter != null) {
+      property.m_sub = topic.subscribe(0, PubSubOption.excludePublisher(property.m_pub));
+      property.m_updateLocal =
+          sub -> {
+            for (long val : sub.readQueueValues()) {
+              setter.accept(val);
+            }
+          };
+    }
+    m_properties.add(property);
+  }
+
+  @Override
+  public void publishConstInteger(String key, long value) {
+    Property<IntegerPublisher, IntegerSubscriber> property = new Property<>();
+    IntegerTopic topic = m_table.getIntegerTopic(key);
+    property.m_pub = topic.publish();
+    property.m_pub.set(value);
+    m_properties.add(property);
+  }
+
+  /**
+   * Add a float property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  @Override
+  public void addFloatProperty(String key, FloatSupplier getter, FloatConsumer setter) {
+    Property<FloatPublisher, FloatSubscriber> property = new Property<>();
+    FloatTopic topic = m_table.getFloatTopic(key);
+    if (getter != null) {
+      property.m_pub = topic.publish();
+      property.m_updateNetwork = (pub, time) -> pub.set(getter.getAsFloat(), time);
+    }
+    if (setter != null) {
+      property.m_sub = topic.subscribe(0.0f, PubSubOption.excludePublisher(property.m_pub));
+      property.m_updateLocal =
+          sub -> {
+            for (float val : sub.readQueueValues()) {
+              setter.accept(val);
+            }
+          };
+    }
+    m_properties.add(property);
+  }
+
+  @Override
+  public void publishConstFloat(String key, float value) {
+    Property<FloatPublisher, FloatSubscriber> property = new Property<>();
+    FloatTopic topic = m_table.getFloatTopic(key);
+    property.m_pub = topic.publish();
+    property.m_pub.set(value);
+    m_properties.add(property);
+  }
+
+  /**
+   * Add a double property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  @Override
+  public void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer setter) {
+    Property<DoublePublisher, DoubleSubscriber> property = new Property<>();
+    DoubleTopic topic = m_table.getDoubleTopic(key);
+    if (getter != null) {
+      property.m_pub = topic.publish();
+      property.m_updateNetwork = (pub, time) -> pub.set(getter.getAsDouble(), time);
+    }
+    if (setter != null) {
+      property.m_sub = topic.subscribe(0.0, PubSubOption.excludePublisher(property.m_pub));
+      property.m_updateLocal =
+          sub -> {
+            for (double val : sub.readQueueValues()) {
+              setter.accept(val);
+            }
+          };
+    }
+    m_properties.add(property);
+  }
+
+  @Override
+  public void publishConstDouble(String key, double value) {
+    Property<DoublePublisher, DoubleSubscriber> property = new Property<>();
+    DoubleTopic topic = m_table.getDoubleTopic(key);
+    property.m_pub = topic.publish();
+    property.m_pub.set(value);
+    m_properties.add(property);
+  }
+
+  /**
+   * Add a string property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  @Override
+  public void addStringProperty(String key, Supplier<String> getter, Consumer<String> setter) {
+    Property<StringPublisher, StringSubscriber> property = new Property<>();
+    StringTopic topic = m_table.getStringTopic(key);
+    if (getter != null) {
+      property.m_pub = topic.publish();
+      property.m_updateNetwork = (pub, time) -> pub.set(getter.get(), time);
+    }
+    if (setter != null) {
+      property.m_sub = topic.subscribe("", PubSubOption.excludePublisher(property.m_pub));
+      property.m_updateLocal =
+          sub -> {
+            for (String val : sub.readQueueValues()) {
+              setter.accept(val);
+            }
+          };
+    }
+    m_properties.add(property);
+  }
+
+  @Override
+  public void publishConstString(String key, String value) {
+    Property<StringPublisher, StringSubscriber> property = new Property<>();
+    StringTopic topic = m_table.getStringTopic(key);
+    property.m_pub = topic.publish();
+    property.m_pub.set(value);
+    m_properties.add(property);
+  }
+
+  /**
+   * Add a boolean array property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  @Override
+  public void addBooleanArrayProperty(
+      String key, Supplier<boolean[]> getter, Consumer<boolean[]> setter) {
+    Property<BooleanArrayPublisher, BooleanArraySubscriber> property = new Property<>();
+    BooleanArrayTopic topic = m_table.getBooleanArrayTopic(key);
+    if (getter != null) {
+      property.m_pub = topic.publish();
+      property.m_updateNetwork = (pub, time) -> pub.set(getter.get(), time);
+    }
+    if (setter != null) {
+      property.m_sub =
+          topic.subscribe(new boolean[] {}, PubSubOption.excludePublisher(property.m_pub));
+      property.m_updateLocal =
+          sub -> {
+            for (boolean[] val : sub.readQueueValues()) {
+              setter.accept(val);
+            }
+          };
+    }
+    m_properties.add(property);
+  }
+
+  @Override
+  public void publishConstBooleanArray(String key, boolean[] value) {
+    Property<BooleanArrayPublisher, BooleanArraySubscriber> property = new Property<>();
+    BooleanArrayTopic topic = m_table.getBooleanArrayTopic(key);
+    property.m_pub = topic.publish();
+    property.m_pub.set(value);
+    m_properties.add(property);
+  }
+
+  /**
+   * Add an integer array property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  @Override
+  public void addIntegerArrayProperty(
+      String key, Supplier<long[]> getter, Consumer<long[]> setter) {
+    Property<IntegerArrayPublisher, IntegerArraySubscriber> property = new Property<>();
+    IntegerArrayTopic topic = m_table.getIntegerArrayTopic(key);
+    if (getter != null) {
+      property.m_pub = topic.publish();
+      property.m_updateNetwork = (pub, time) -> pub.set(getter.get(), time);
+    }
+    if (setter != null) {
+      property.m_sub =
+          topic.subscribe(new long[] {}, PubSubOption.excludePublisher(property.m_pub));
+      property.m_updateLocal =
+          sub -> {
+            for (long[] val : sub.readQueueValues()) {
+              setter.accept(val);
+            }
+          };
+    }
+    m_properties.add(property);
+  }
+
+  @Override
+  public void publishConstIntegerArray(String key, long[] value) {
+    Property<IntegerArrayPublisher, IntegerArraySubscriber> property = new Property<>();
+    IntegerArrayTopic topic = m_table.getIntegerArrayTopic(key);
+    property.m_pub = topic.publish();
+    property.m_pub.set(value);
+    m_properties.add(property);
+  }
+
+  /**
+   * Add a float array property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  @Override
+  public void addFloatArrayProperty(
+      String key, Supplier<float[]> getter, Consumer<float[]> setter) {
+    Property<FloatArrayPublisher, FloatArraySubscriber> property = new Property<>();
+    FloatArrayTopic topic = m_table.getFloatArrayTopic(key);
+    if (getter != null) {
+      property.m_pub = topic.publish();
+      property.m_updateNetwork = (pub, time) -> pub.set(getter.get(), time);
+    }
+    if (setter != null) {
+      property.m_sub =
+          topic.subscribe(new float[] {}, PubSubOption.excludePublisher(property.m_pub));
+      property.m_updateLocal =
+          sub -> {
+            for (float[] val : sub.readQueueValues()) {
+              setter.accept(val);
+            }
+          };
+    }
+    m_properties.add(property);
+  }
+
+  @Override
+  public void publishConstFloatArray(String key, float[] value) {
+    Property<FloatArrayPublisher, FloatArraySubscriber> property = new Property<>();
+    FloatArrayTopic topic = m_table.getFloatArrayTopic(key);
+    property.m_pub = topic.publish();
+    property.m_pub.set(value);
+    m_properties.add(property);
+  }
+
+  /**
+   * Add a double array property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  @Override
+  public void addDoubleArrayProperty(
+      String key, Supplier<double[]> getter, Consumer<double[]> setter) {
+    Property<DoubleArrayPublisher, DoubleArraySubscriber> property = new Property<>();
+    DoubleArrayTopic topic = m_table.getDoubleArrayTopic(key);
+    if (getter != null) {
+      property.m_pub = topic.publish();
+      property.m_updateNetwork = (pub, time) -> pub.set(getter.get(), time);
+    }
+    if (setter != null) {
+      property.m_sub =
+          topic.subscribe(new double[] {}, PubSubOption.excludePublisher(property.m_pub));
+      property.m_updateLocal =
+          sub -> {
+            for (double[] val : sub.readQueueValues()) {
+              setter.accept(val);
+            }
+          };
+    }
+    m_properties.add(property);
+  }
+
+  @Override
+  public void publishConstDoubleArray(String key, double[] value) {
+    Property<DoubleArrayPublisher, DoubleArraySubscriber> property = new Property<>();
+    DoubleArrayTopic topic = m_table.getDoubleArrayTopic(key);
+    property.m_pub = topic.publish();
+    property.m_pub.set(value);
+    m_properties.add(property);
+  }
+
+  /**
+   * Add a string array property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  @Override
+  public void addStringArrayProperty(
+      String key, Supplier<String[]> getter, Consumer<String[]> setter) {
+    Property<StringArrayPublisher, StringArraySubscriber> property = new Property<>();
+    StringArrayTopic topic = m_table.getStringArrayTopic(key);
+    if (getter != null) {
+      property.m_pub = topic.publish();
+      property.m_updateNetwork = (pub, time) -> pub.set(getter.get(), time);
+    }
+    if (setter != null) {
+      property.m_sub =
+          topic.subscribe(new String[] {}, PubSubOption.excludePublisher(property.m_pub));
+      property.m_updateLocal =
+          sub -> {
+            for (String[] val : sub.readQueueValues()) {
+              setter.accept(val);
+            }
+          };
+    }
+    m_properties.add(property);
+  }
+
+  @Override
+  public void publishConstStringArray(String key, String[] value) {
+    Property<StringArrayPublisher, StringArraySubscriber> property = new Property<>();
+    StringArrayTopic topic = m_table.getStringArrayTopic(key);
+    property.m_pub = topic.publish();
+    property.m_pub.set(value);
+    m_properties.add(property);
+  }
+
+  /**
+   * Add a raw property.
+   *
+   * @param key property name
+   * @param typeString type string
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  @Override
+  public void addRawProperty(
+      String key, String typeString, Supplier<byte[]> getter, Consumer<byte[]> setter) {
+    Property<RawPublisher, RawSubscriber> property = new Property<>();
+    RawTopic topic = m_table.getRawTopic(key);
+    if (getter != null) {
+      property.m_pub = topic.publish(typeString);
+      property.m_updateNetwork = (pub, time) -> pub.set(getter.get(), time);
+    }
+    if (setter != null) {
+      property.m_sub =
+          topic.subscribe(typeString, new byte[] {}, PubSubOption.excludePublisher(property.m_pub));
+      property.m_updateLocal =
+          sub -> {
+            for (byte[] val : sub.readQueueValues()) {
+              setter.accept(val);
+            }
+          };
+    }
+    m_properties.add(property);
+  }
+
+  @Override
+  public void publishConstRaw(String key, String typestring, byte[] value) {
+    Property<RawPublisher, RawSubscriber> property = new Property<>();
+    RawTopic topic = m_table.getRawTopic(key);
+    property.m_pub = topic.publish(typestring);
+    property.m_pub.set(value);
+    m_properties.add(property);
+  }
+}

wpilibj/src/main/java/org/wpilib/smartdashboard/SendableChooser.java

@@ -0,0 +1,174 @@
+// 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 edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.LinkedHashMap;
+import java.util.Map;
+import java.util.concurrent.atomic.AtomicInteger;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Consumer;
+
+/**
+ * The {@link SendableChooser} class is a useful tool for presenting a selection of options to the
+ * {@link SmartDashboard}.
+ *
+ * <p>For instance, you may wish to be able to select between multiple autonomous modes. You can do
+ * this by putting every possible Command you want to run as an autonomous into a {@link
+ * SendableChooser} and then put it into the {@link SmartDashboard} to have a list of options appear
+ * on the laptop. Once autonomous starts, simply ask the {@link SendableChooser} what the selected
+ * value is.
+ *
+ * @param <V> The type of the values to be stored
+ */
+public class SendableChooser<V> implements Sendable, AutoCloseable {
+  /** The key for the default value. */
+  private static final String DEFAULT = "default";
+
+  /** The key for the selected option. */
+  private static final String SELECTED = "selected";
+
+  /** The key for the active option. */
+  private static final String ACTIVE = "active";
+
+  /** The key for the option array. */
+  private static final String OPTIONS = "options";
+
+  /** The key for the instance number. */
+  private static final String INSTANCE = ".instance";
+
+  /** A map linking strings to the objects they represent. */
+  private final Map<String, V> m_map = new LinkedHashMap<>();
+
+  private String m_defaultChoice = "";
+  private final int m_instance;
+  private String m_previousVal;
+  private Consumer<V> m_listener;
+  private static final AtomicInteger s_instances = new AtomicInteger();
+
+  /** Instantiates a {@link SendableChooser}. */
+  @SuppressWarnings("this-escape")
+  public SendableChooser() {
+    m_instance = s_instances.getAndIncrement();
+    SendableRegistry.add(this, "SendableChooser", m_instance);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the
+   * object will appear as the given name.
+   *
+   * @param name the name of the option
+   * @param object the option
+   */
+  public void addOption(String name, V object) {
+    m_map.put(name, object);
+  }
+
+  /**
+   * Adds the given object to the list of options and marks it as the default. Functionally, this is
+   * very close to {@link #addOption(String, Object)} except that it will use this as the default
+   * option if none other is explicitly selected.
+   *
+   * @param name the name of the option
+   * @param object the option
+   */
+  public void setDefaultOption(String name, V object) {
+    requireNonNullParam(name, "name", "setDefaultOption");
+
+    m_defaultChoice = name;
+    addOption(name, object);
+  }
+
+  /**
+   * Returns the selected option. If there is none selected, it will return the default. If there is
+   * none selected and no default, then it will return {@code null}.
+   *
+   * @return the option selected
+   */
+  public V getSelected() {
+    m_mutex.lock();
+    try {
+      if (m_selected != null) {
+        return m_map.get(m_selected);
+      } else {
+        return m_map.get(m_defaultChoice);
+      }
+    } finally {
+      m_mutex.unlock();
+    }
+  }
+
+  /**
+   * Bind a listener that's called when the selected value changes. Only one listener can be bound.
+   * Calling this function will replace the previous listener.
+   *
+   * @param listener The function to call that accepts the new value
+   */
+  public void onChange(Consumer<V> listener) {
+    requireNonNullParam(listener, "listener", "onChange");
+    m_mutex.lock();
+    m_listener = listener;
+    m_mutex.unlock();
+  }
+
+  private String m_selected;
+  private final ReentrantLock m_mutex = new ReentrantLock();
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("String Chooser");
+    builder.publishConstInteger(INSTANCE, m_instance);
+    builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null);
+    builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null);
+    builder.addStringProperty(
+        ACTIVE,
+        () -> {
+          m_mutex.lock();
+          try {
+            if (m_selected != null) {
+              return m_selected;
+            } else {
+              return m_defaultChoice;
+            }
+          } finally {
+            m_mutex.unlock();
+          }
+        },
+        null);
+    builder.addStringProperty(
+        SELECTED,
+        null,
+        val -> {
+          V choice;
+          Consumer<V> listener;
+          m_mutex.lock();
+          try {
+            m_selected = val;
+            if (!m_selected.equals(m_previousVal) && m_listener != null) {
+              choice = m_map.get(val);
+              listener = m_listener;
+            } else {
+              choice = null;
+              listener = null;
+            }
+            m_previousVal = val;
+          } finally {
+            m_mutex.unlock();
+          }
+          if (listener != null) {
+            listener.accept(choice);
+          }
+        });
+  }
+}

wpilibj/src/main/java/org/wpilib/smartdashboard/SmartDashboard.java

@@ -0,0 +1,517 @@
+// 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.hal.HAL;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.Set;
+
+/**
+ * The {@link SmartDashboard} class is the bridge between robot programs and the SmartDashboard on
+ * the laptop.
+ *
+ * <p>When a value is put into the SmartDashboard here, it pops up on the SmartDashboard on the
+ * laptop. Users can put values into and get values from the SmartDashboard.
+ */
+public final class SmartDashboard {
+  /** The {@link NetworkTable} used by {@link SmartDashboard}. */
+  private static NetworkTable table;
+
+  /**
+   * A table linking tables in the SmartDashboard to the {@link Sendable} objects they came from.
+   */
+  private static final Map<String, Sendable> tablesToData = new HashMap<>();
+
+  /** The executor for listener tasks; calls listener tasks synchronously from main thread. */
+  private static final ListenerExecutor listenerExecutor = new ListenerExecutor();
+
+  private static boolean m_reported = false;
+
+  static {
+    setNetworkTableInstance(NetworkTableInstance.getDefault());
+  }
+
+  private SmartDashboard() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Set the NetworkTable instance used for entries. For testing purposes; use with caution.
+   *
+   * @param inst NetworkTable instance
+   */
+  public static synchronized void setNetworkTableInstance(NetworkTableInstance inst) {
+    SmartDashboard.table = inst.getTable("SmartDashboard");
+    tablesToData.clear();
+  }
+
+  /**
+   * Maps the specified key to the specified value in this table. The key can not be null. The value
+   * can be retrieved by calling the get method with a key that is equal to the original key.
+   *
+   * @param key the key
+   * @param data the value
+   * @throws IllegalArgumentException If key is null
+   */
+  @SuppressWarnings("PMD.CompareObjectsWithEquals")
+  public static synchronized void putData(String key, Sendable data) {
+    if (!m_reported) {
+      HAL.reportUsage("SmartDashboard", "");
+      m_reported = true;
+    }
+    Sendable sddata = tablesToData.get(key);
+    if (sddata == null || sddata != data) {
+      tablesToData.put(key, data);
+      NetworkTable dataTable = table.getSubTable(key);
+      SendableBuilderImpl builder = new SendableBuilderImpl();
+      builder.setTable(dataTable);
+      SendableRegistry.publish(data, builder);
+      builder.startListeners();
+      dataTable.getEntry(".name").setString(key);
+    }
+  }
+
+  /**
+   * Maps the specified key (where the key is the name of the {@link Sendable}) to the specified
+   * value in this table. The value can be retrieved by calling the get method with a key that is
+   * equal to the original key.
+   *
+   * @param value the value
+   * @throws IllegalArgumentException If key is null
+   */
+  public static void putData(Sendable value) {
+    String name = SendableRegistry.getName(value);
+    if (!name.isEmpty()) {
+      putData(name, value);
+    }
+  }
+
+  /**
+   * Returns the value at the specified key.
+   *
+   * @param key the key
+   * @return the value
+   * @throws IllegalArgumentException if the key is null
+   */
+  public static synchronized Sendable getData(String key) {
+    Sendable data = tablesToData.get(key);
+    if (data == null) {
+      throw new IllegalArgumentException("SmartDashboard data does not exist: " + key);
+    } else {
+      return data;
+    }
+  }
+
+  /**
+   * Gets the entry for the specified key.
+   *
+   * @param key the key name
+   * @return Network table entry.
+   */
+  public static NetworkTableEntry getEntry(String key) {
+    if (!m_reported) {
+      HAL.reportUsage("SmartDashboard", "");
+      m_reported = true;
+    }
+    return table.getEntry(key);
+  }
+
+  /**
+   * Checks the table and tells if it contains the specified key.
+   *
+   * @param key the key to search for
+   * @return true if the table as a value assigned to the given key
+   */
+  public static boolean containsKey(String key) {
+    return table.containsKey(key);
+  }
+
+  /**
+   * Get the keys stored in the SmartDashboard table of NetworkTables.
+   *
+   * @param types bitmask of types; 0 is treated as a "don't care".
+   * @return keys currently in the table
+   */
+  public static Set<String> getKeys(int types) {
+    return table.getKeys(types);
+  }
+
+  /**
+   * Get the keys stored in the SmartDashboard table of NetworkTables.
+   *
+   * @return keys currently in the table.
+   */
+  public static Set<String> getKeys() {
+    return table.getKeys();
+  }
+
+  /**
+   * Makes a key's value persistent through program restarts. The key cannot be null.
+   *
+   * @param key the key name
+   */
+  public static void setPersistent(String key) {
+    getEntry(key).setPersistent();
+  }
+
+  /**
+   * Stop making a key's value persistent through program restarts. The key cannot be null.
+   *
+   * @param key the key name
+   */
+  public static void clearPersistent(String key) {
+    getEntry(key).clearPersistent();
+  }
+
+  /**
+   * Returns whether the value is persistent through program restarts. The key cannot be null.
+   *
+   * @param key the key name
+   * @return True if the value is persistent.
+   */
+  public static boolean isPersistent(String key) {
+    return getEntry(key).isPersistent();
+  }
+
+  /**
+   * Put a boolean in the table.
+   *
+   * @param key the key to be assigned to
+   * @param value the value that will be assigned
+   * @return False if the table key already exists with a different type
+   */
+  public static boolean putBoolean(String key, boolean value) {
+    return getEntry(key).setBoolean(value);
+  }
+
+  /**
+   * Set the value in the table if key does not exist.
+   *
+   * @param key the key
+   * @param defaultValue the value to set if key does not exist
+   * @return True if the key did not already exist, otherwise False
+   */
+  public static boolean setDefaultBoolean(String key, boolean defaultValue) {
+    return getEntry(key).setDefaultBoolean(defaultValue);
+  }
+
+  /**
+   * Returns the boolean the key maps to. If the key does not exist or is of different type, it will
+   * return the default value.
+   *
+   * @param key the key to look up
+   * @param defaultValue the value to be returned if no value is found
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
+   */
+  public static boolean getBoolean(String key, boolean defaultValue) {
+    return getEntry(key).getBoolean(defaultValue);
+  }
+
+  /**
+   * Put a number in the table.
+   *
+   * @param key the key to be assigned to
+   * @param value the value that will be assigned
+   * @return False if the table key already exists with a different type
+   */
+  public static boolean putNumber(String key, double value) {
+    return getEntry(key).setDouble(value);
+  }
+
+  /**
+   * Set the value in the table if key does not exist.
+   *
+   * @param key the key
+   * @param defaultValue the value to set if key does not exist
+   * @return True if the key did not already exist, otherwise False
+   */
+  public static boolean setDefaultNumber(String key, double defaultValue) {
+    return getEntry(key).setDefaultDouble(defaultValue);
+  }
+
+  /**
+   * Returns the number the key maps to. If the key does not exist or is of different type, it will
+   * return the default value.
+   *
+   * @param key the key to look up
+   * @param defaultValue the value to be returned if no value is found
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
+   */
+  public static double getNumber(String key, double defaultValue) {
+    return getEntry(key).getDouble(defaultValue);
+  }
+
+  /**
+   * Put a string in the table.
+   *
+   * @param key the key to be assigned to
+   * @param value the value that will be assigned
+   * @return False if the table key already exists with a different type
+   */
+  public static boolean putString(String key, String value) {
+    return getEntry(key).setString(value);
+  }
+
+  /**
+   * Set the value in the table if key does not exist.
+   *
+   * @param key the key
+   * @param defaultValue the value to set if key does not exist
+   * @return True if the key did not already exist, otherwise False
+   */
+  public static boolean setDefaultString(String key, String defaultValue) {
+    return getEntry(key).setDefaultString(defaultValue);
+  }
+
+  /**
+   * Returns the string the key maps to. If the key does not exist or is of different type, it will
+   * return the default value.
+   *
+   * @param key the key to look up
+   * @param defaultValue the value to be returned if no value is found
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
+   */
+  public static String getString(String key, String defaultValue) {
+    return getEntry(key).getString(defaultValue);
+  }
+
+  /**
+   * Put a boolean array in the table.
+   *
+   * @param key the key to be assigned to
+   * @param value the value that will be assigned
+   * @return False if the table key already exists with a different type
+   */
+  public static boolean putBooleanArray(String key, boolean[] value) {
+    return getEntry(key).setBooleanArray(value);
+  }
+
+  /**
+   * Put a boolean array in the table.
+   *
+   * @param key the key to be assigned to
+   * @param value the value that will be assigned
+   * @return False if the table key already exists with a different type
+   */
+  public static boolean putBooleanArray(String key, Boolean[] value) {
+    return getEntry(key).setBooleanArray(value);
+  }
+
+  /**
+   * Set the value in the table if key does not exist.
+   *
+   * @param key the key
+   * @param defaultValue the value to set if key does not exist
+   * @return True if the key did not already exist, otherwise False
+   */
+  public static boolean setDefaultBooleanArray(String key, boolean[] defaultValue) {
+    return getEntry(key).setDefaultBooleanArray(defaultValue);
+  }
+
+  /**
+   * Set the value in the table if key does not exist.
+   *
+   * @param key the key
+   * @param defaultValue the value to set if key does not exist
+   * @return True if the key did not already exist, otherwise False
+   */
+  public static boolean setDefaultBooleanArray(String key, Boolean[] defaultValue) {
+    return getEntry(key).setDefaultBooleanArray(defaultValue);
+  }
+
+  /**
+   * Returns the boolean array the key maps to. If the key does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param key the key to look up
+   * @param defaultValue the value to be returned if no value is found
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
+   */
+  public static boolean[] getBooleanArray(String key, boolean[] defaultValue) {
+    return getEntry(key).getBooleanArray(defaultValue);
+  }
+
+  /**
+   * Returns the boolean array the key maps to. If the key does not exist or is of different type,
+   * it will return the default value.
+   *
+   * @param key the key to look up
+   * @param defaultValue the value to be returned if no value is found
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
+   */
+  public static Boolean[] getBooleanArray(String key, Boolean[] defaultValue) {
+    return getEntry(key).getBooleanArray(defaultValue);
+  }
+
+  /**
+   * Put a number array in the table.
+   *
+   * @param key the key to be assigned to
+   * @param value the value that will be assigned
+   * @return False if the table key already exists with a different type
+   */
+  public static boolean putNumberArray(String key, double[] value) {
+    return getEntry(key).setDoubleArray(value);
+  }
+
+  /**
+   * Put a number array in the table.
+   *
+   * @param key the key to be assigned to
+   * @param value the value that will be assigned
+   * @return False if the table key already exists with a different type
+   */
+  public static boolean putNumberArray(String key, Double[] value) {
+    return getEntry(key).setNumberArray(value);
+  }
+
+  /**
+   * Set the value in the table if key does not exist.
+   *
+   * @param key the key
+   * @param defaultValue the value to set if key does not exist
+   * @return True if the key did not already exist, otherwise False
+   */
+  public static boolean setDefaultNumberArray(String key, double[] defaultValue) {
+    return getEntry(key).setDefaultDoubleArray(defaultValue);
+  }
+
+  /**
+   * Set the value in the table if key does not exist.
+   *
+   * @param key the key
+   * @param defaultValue the value to set if key does not exist
+   * @return True if the key did not already exist, otherwise False
+   */
+  public static boolean setDefaultNumberArray(String key, Double[] defaultValue) {
+    return getEntry(key).setDefaultNumberArray(defaultValue);
+  }
+
+  /**
+   * Returns the number array the key maps to. If the key does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param key the key to look up
+   * @param defaultValue the value to be returned if no value is found
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
+   */
+  public static double[] getNumberArray(String key, double[] defaultValue) {
+    return getEntry(key).getDoubleArray(defaultValue);
+  }
+
+  /**
+   * Returns the number array the key maps to. If the key does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param key the key to look up
+   * @param defaultValue the value to be returned if no value is found
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
+   */
+  public static Double[] getNumberArray(String key, Double[] defaultValue) {
+    return getEntry(key).getDoubleArray(defaultValue);
+  }
+
+  /**
+   * Put a string array in the table.
+   *
+   * @param key the key to be assigned to
+   * @param value the value that will be assigned
+   * @return False if the table key already exists with a different type
+   */
+  public static boolean putStringArray(String key, String[] value) {
+    return getEntry(key).setStringArray(value);
+  }
+
+  /**
+   * Set the value in the table if key does not exist.
+   *
+   * @param key the key
+   * @param defaultValue the value to set if key does not exist
+   * @return True if the key did not already exist, otherwise False
+   */
+  public static boolean setDefaultStringArray(String key, String[] defaultValue) {
+    return getEntry(key).setDefaultStringArray(defaultValue);
+  }
+
+  /**
+   * Returns the string array the key maps to. If the key does not exist or is of different type, it
+   * will return the default value.
+   *
+   * @param key the key to look up
+   * @param defaultValue the value to be returned if no value is found
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
+   */
+  public static String[] getStringArray(String key, String[] defaultValue) {
+    return getEntry(key).getStringArray(defaultValue);
+  }
+
+  /**
+   * Put a raw value (byte array) in the table.
+   *
+   * @param key the key to be assigned to
+   * @param value the value that will be assigned
+   * @return False if the table key already exists with a different type
+   */
+  public static boolean putRaw(String key, byte[] value) {
+    return getEntry(key).setRaw(value);
+  }
+
+  /**
+   * Set the value in the table if key does not exist.
+   *
+   * @param key the key
+   * @param defaultValue the value to set if key does not exist
+   * @return True if the key did not already exist, otherwise False
+   */
+  public static boolean setDefaultRaw(String key, byte[] defaultValue) {
+    return getEntry(key).setDefaultRaw(defaultValue);
+  }
+
+  /**
+   * Returns the raw value (byte array) the key maps to. If the key does not exist or is of
+   * different type, it will return the default value.
+   *
+   * @param key the key to look up
+   * @param defaultValue the value to be returned if no value is found
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
+   */
+  public static byte[] getRaw(String key, byte[] defaultValue) {
+    return getEntry(key).getRaw(defaultValue);
+  }
+
+  /**
+   * Posts a task from a listener to the ListenerExecutor, so that it can be run synchronously from
+   * the main loop on the next call to {@link SmartDashboard#updateValues()}.
+   *
+   * @param task The task to run synchronously from the main thread.
+   */
+  public static void postListenerTask(Runnable task) {
+    listenerExecutor.execute(task);
+  }
+
+  /** Puts all sendable data to the dashboard. */
+  public static synchronized void updateValues() {
+    // Execute posted listener tasks
+    listenerExecutor.runListenerTasks();
+    for (Sendable data : tablesToData.values()) {
+      SendableRegistry.update(data);
+    }
+  }
+}