[wpilib] Move Field2d to SmartDashboard

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

@@ -1,49 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/simulation/Field2d.h"
-
-using namespace frc;
-
-Field2d::Field2d() : m_device{"Field2D"} {
-  if (m_device) {
-    m_x = m_device.CreateDouble("x", false, 0.0);
-    m_y = m_device.CreateDouble("y", false, 0.0);
-    m_rot = m_device.CreateDouble("rot", false, 0.0);
-  }
-}
-
-void Field2d::SetRobotPose(const Pose2d& pose) {
-  if (m_device) {
-    auto& translation = pose.Translation();
-    m_x.Set(translation.X().to<double>());
-    m_y.Set(translation.Y().to<double>());
-    m_rot.Set(pose.Rotation().Degrees().to<double>());
-  } else {
-    m_pose = pose;
-  }
-}
-
-void Field2d::SetRobotPose(units::meter_t x, units::meter_t y,
-                           Rotation2d rotation) {
-  if (m_device) {
-    m_x.Set(x.to<double>());
-    m_y.Set(y.to<double>());
-    m_rot.Set(rotation.Degrees().to<double>());
-  } else {
-    m_pose = Pose2d{x, y, rotation};
-  }
-}
-
-Pose2d Field2d::GetRobotPose() {
-  if (m_device) {
-    return Pose2d{units::meter_t{m_x.Get()}, units::meter_t{m_y.Get()},
-                  Rotation2d{units::degree_t{m_rot.Get()}}};
-  } else {
-    return m_pose;
-  }
-}

wpilibc/src/main/native/cpp/smartdashboard/Field2d.cpp

@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/Field2d.h"
+
+#include "frc/smartdashboard/SendableBuilder.h"
+
+using namespace frc;
+
+Field2d::Field2d() {
+  m_objects.emplace_back(
+      std::make_unique<FieldObject2d>("Robot", FieldObject2d::private_init{}));
+  m_objects[0]->SetPose(Pose2d{});
+}
+
+void Field2d::SetRobotPose(const Pose2d& pose) {
+  std::scoped_lock lock(m_mutex);
+  m_objects[0]->SetPose(pose);
+}
+
+void Field2d::SetRobotPose(units::meter_t x, units::meter_t y,
+                           Rotation2d rotation) {
+  std::scoped_lock lock(m_mutex);
+  m_objects[0]->SetPose(x, y, rotation);
+}
+
+Pose2d Field2d::GetRobotPose() const {
+  std::scoped_lock lock(m_mutex);
+  return m_objects[0]->GetPose();
+}
+
+FieldObject2d* Field2d::GetObject(const wpi::Twine& name) {
+  std::scoped_lock lock(m_mutex);
+  std::string nameStr = name.str();
+  for (auto&& obj : m_objects) {
+    if (obj->m_name == nameStr) return obj.get();
+  }
+  m_objects.emplace_back(std::make_unique<FieldObject2d>(
+      std::move(nameStr), FieldObject2d::private_init{}));
+  auto obj = m_objects.back().get();
+  if (m_table) obj->m_entry = m_table->GetEntry(obj->m_name);
+  return obj;
+}
+
+FieldObject2d* Field2d::GetRobotObject() {
+  std::scoped_lock lock(m_mutex);
+  return m_objects[0].get();
+}
+
+void Field2d::InitSendable(SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Field2d");
+  m_table = builder.GetTable();
+
+  std::scoped_lock lock(m_mutex);
+  for (auto&& obj : m_objects) {
+    std::scoped_lock lock2(obj->m_mutex);
+    obj->m_entry = m_table->GetEntry(obj->m_name);
+    obj->UpdateEntry(true);
+  }
+}

wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp

@@ -0,0 +1,80 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/smartdashboard/FieldObject2d.h"
+
+using namespace frc;
+
+void FieldObject2d::SetPose(const Pose2d& pose) {
+  SetPoses(wpi::makeArrayRef(pose));
+}
+
+void FieldObject2d::SetPose(units::meter_t x, units::meter_t y,
+                            Rotation2d rotation) {
+  SetPoses(wpi::makeArrayRef(Pose2d{x, y, rotation}));
+}
+
+Pose2d FieldObject2d::GetPose() const {
+  std::scoped_lock lock(m_mutex);
+  UpdateFromEntry();
+  if (m_poses.empty()) return {};
+  return m_poses[0];
+}
+
+void FieldObject2d::SetPoses(wpi::ArrayRef<Pose2d> poses) {
+  std::scoped_lock lock(m_mutex);
+  m_poses.assign(poses.begin(), poses.end());
+  UpdateEntry();
+}
+
+void FieldObject2d::SetPoses(std::initializer_list<Pose2d> poses) {
+  SetPoses(wpi::makeArrayRef(poses.begin(), poses.end()));
+}
+
+std::vector<Pose2d> FieldObject2d::GetPoses() const {
+  std::scoped_lock lock(m_mutex);
+  UpdateFromEntry();
+  return std::vector<Pose2d>(m_poses.begin(), m_poses.end());
+}
+
+wpi::ArrayRef<Pose2d> FieldObject2d::GetPoses(
+    wpi::SmallVectorImpl<Pose2d>& out) const {
+  std::scoped_lock lock(m_mutex);
+  UpdateFromEntry();
+  out.assign(m_poses.begin(), m_poses.end());
+  return out;
+}
+
+void FieldObject2d::UpdateEntry(bool setDefault) {
+  if (!m_entry) return;
+  wpi::SmallVector<double, 9> arr;
+  for (auto&& pose : m_poses) {
+    auto& translation = pose.Translation();
+    arr.push_back(translation.X().to<double>());
+    arr.push_back(translation.Y().to<double>());
+    arr.push_back(pose.Rotation().Degrees().to<double>());
+  }
+  if (setDefault)
+    m_entry.SetDefaultDoubleArray(arr);
+  else
+    m_entry.SetDoubleArray(arr);
+}
+
+void FieldObject2d::UpdateFromEntry() const {
+  if (!m_entry) return;
+  auto val = m_entry.GetValue();
+  if (!val || !val->IsDoubleArray()) return;
+  auto arr = val->GetDoubleArray();
+  auto size = arr.size();
+  if ((size % 3) != 0) return;
+  m_poses.resize(size / 3);
+  for (size_t i = 0; i < size / 3; ++i) {
+    m_poses[i] =
+        Pose2d{units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
+               Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
+  }
+}

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

@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <hal/SimDevice.h>
-#include <units/length.h>
-
-#include "frc/geometry/Pose2d.h"
-#include "frc/geometry/Rotation2d.h"
-
-namespace frc {
-
-/**
- * 2D representation of game field (for simulation).
- *
- * In non-simulation mode this simply stores and returns the robot pose.
- *
- * The robot pose is the actual location shown on the simulation view.
- * This may or may not match the robot's internal odometry.  For example, if
- * 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).
- *
- * 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 simulated wheel
- * velocity), and set the new pose.
- */
-class Field2d {
- public:
-  Field2d();
-
-  /**
-   * Set the robot pose from a Pose object.
-   *
-   * @param pose 2D pose
-   */
-  void SetRobotPose(const Pose2d& pose);
-
-  /**
-   * Set the robot pose from x, y, and rotation.
-   *
-   * @param x X location
-   * @param y Y location
-   * @param rotation rotation
-   */
-  void SetRobotPose(units::meter_t x, units::meter_t y, Rotation2d rotation);
-
-  /**
-   * Get the robot pose.
-   *
-   * @return 2D pose
-   */
-  Pose2d GetRobotPose();
-
- private:
-  Pose2d m_pose;
-
-  hal::SimDevice m_device;
-  hal::SimDouble m_x;
-  hal::SimDouble m_y;
-  hal::SimDouble m_rot;
-};
-
-}  // namespace frc

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

@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <vector>
+
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <units/length.h>
+#include <wpi/Twine.h>
+#include <wpi/mutex.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/smartdashboard/FieldObject2d.h"
+#include "frc/smartdashboard/Sendable.h"
+
+namespace frc {
+
+/**
+ * 2D representation of game field for dashboards.
+ *
+ * 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,
+ * if 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).
+ *
+ * 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.
+ *
+ * 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).
+ */
+class Field2d : public Sendable {
+ public:
+  using Entry = size_t;
+
+  Field2d();
+
+  /**
+   * Set the robot pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  void SetRobotPose(const Pose2d& pose);
+
+  /**
+   * Set the robot pose from x, y, and rotation.
+   *
+   * @param x X location
+   * @param y Y location
+   * @param rotation rotation
+   */
+  void SetRobotPose(units::meter_t x, units::meter_t y, Rotation2d rotation);
+
+  /**
+   * Get the robot pose.
+   *
+   * @return 2D pose
+   */
+  Pose2d GetRobotPose() const;
+
+  /**
+   * Get or create a field object.
+   *
+   * @return Field object
+   */
+  FieldObject2d* GetObject(const wpi::Twine& name);
+
+  /**
+   * Get the robot object.
+   *
+   * @return Field object for robot
+   */
+  FieldObject2d* GetRobotObject();
+
+  void InitSendable(SendableBuilder& builder) override;
+
+ private:
+  std::shared_ptr<nt::NetworkTable> m_table;
+
+  mutable wpi::mutex m_mutex;
+  std::vector<std::unique_ptr<FieldObject2d>> m_objects;
+};
+
+}  // namespace frc

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

@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <initializer_list>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <networktables/NetworkTableEntry.h>
+#include <units/length.h>
+#include <wpi/ArrayRef.h>
+#include <wpi/SmallVector.h>
+#include <wpi/mutex.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+
+class Field2d;
+
+/**
+ * Game field object on a Field2d.
+ */
+class FieldObject2d {
+  friend class Field2d;
+  struct private_init {};
+
+ public:
+  FieldObject2d(std::string&& name, const private_init&)
+      : m_name{std::move(name)} {}
+
+  /**
+   * Set the pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  void SetPose(const Pose2d& pose);
+
+  /**
+   * Set the pose from x, y, and rotation.
+   *
+   * @param x X location
+   * @param y Y location
+   * @param rotation rotation
+   */
+  void SetPose(units::meter_t x, units::meter_t y, Rotation2d rotation);
+
+  /**
+   * Get the pose.
+   *
+   * @return 2D pose, or 0,0,0 if unknown / does not exist
+   */
+  Pose2d GetPose() const;
+
+  /**
+   * Set multiple poses from an array of Pose objects.
+   * The total number of poses is limited to 85.
+   *
+   * @param poses array of 2D poses
+   */
+  void SetPoses(wpi::ArrayRef<Pose2d> poses);
+
+  /**
+   * Set multiple poses from an array of Pose objects.
+   * The total number of poses is limited to 85.
+   *
+   * @param obj Object entry
+   * @param poses array of 2D poses
+   */
+  void SetPoses(std::initializer_list<Pose2d> poses);
+
+  /**
+   * Get multiple poses.
+   *
+   * @param obj Object entry
+   * @return vector of 2D poses
+   */
+  std::vector<Pose2d> GetPoses() const;
+
+  /**
+   * Get multiple poses.
+   *
+   * @param obj Object entry
+   * @param out output SmallVector to hold 2D poses
+   * @return ArrayRef referring to output SmallVector
+   */
+  wpi::ArrayRef<Pose2d> GetPoses(wpi::SmallVectorImpl<Pose2d>& out) const;
+
+ private:
+  void UpdateEntry(bool setDefault = false);
+  void UpdateFromEntry() const;
+
+  mutable wpi::mutex m_mutex;
+  std::string m_name;
+  nt::NetworkTableEntry m_entry;
+  mutable wpi::SmallVector<Pose2d, 1> m_poses;
+};
+
+}  // namespace frc

wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp

@@ -12,6 +12,7 @@
 #include <frc/geometry/Rotation2d.h>
 #include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
 #include <frc/simulation/SimDeviceSim.h>
+#include <frc/smartdashboard/SmartDashboard.h>
 
 using namespace DriveConstants;
 
@@ -53,6 +54,7 @@ void DriveSubsystem::SimulationPeriodic() {
       -m_drivetrainSimulator.GetHeading().Degrees().to<double>());
 
   m_fieldSim.SetRobotPose(m_odometry.GetPose());
+  frc::SmartDashboard::PutData("Field", &m_fieldSim);
 }
 
 units::ampere_t DriveSubsystem::GetCurrentDraw() const {

wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h

@@ -17,7 +17,7 @@
 #include <frc/simulation/AnalogGyroSim.h>
 #include <frc/simulation/DifferentialDrivetrainSim.h>
 #include <frc/simulation/EncoderSim.h>
-#include <frc/simulation/Field2d.h>
+#include <frc/smartdashboard/Field2d.h>
 #include <frc2/command/SubsystemBase.h>
 #include <units/voltage.h>
 
@@ -169,7 +169,6 @@ class DriveSubsystem : public frc2::SubsystemBase {
   frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
   frc::sim::AnalogGyroSim m_gyroAngleSim{m_gyro};
 
-  // The Field2d class simulates the field in the sim GUI. Note that we can have
-  // only one instance!
+  // The Field2d class shows the field in the sim GUI.
   frc::Field2d m_fieldSim;
 };

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

@@ -24,7 +24,7 @@ import edu.wpi.first.wpiutil.math.numbers.N7;
 /**
  * This class simulates the state of the drivetrain. In simulationPeriodic, users should first set inputs from motors with
  * {@link #setInputs(double, double)}, call {@link #update(double)} to update the simulation,
- * and set estimated encoder and gyro positions, as well as estimated odometry pose. Teams can use {@link edu.wpi.first.wpilibj.simulation.Field2d} to
+ * and set estimated encoder and gyro positions, as well as estimated odometry pose. Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to
  * visualize their robot on the Sim GUI's field.
  *
  *  <p>Our state-space system is:

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

@@ -1,100 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.simulation;
-
-import edu.wpi.first.hal.SimDevice;
-import edu.wpi.first.hal.SimDouble;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-/**
- * 2D representation of game field (for simulation).
- *
- * <p>In non-simulation mode this simply stores and returns the robot pose.
- *
- * <p>The robot pose is the actual location shown on the simulation view.
- * This may or may not match the robot's internal odometry.  For example, if
- * 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 simulated wheel
- * velocity), and set the new pose.
- */
-public class Field2d {
-  public Field2d() {
-    m_device = SimDevice.create("Field2D");
-    if (m_device != null) {
-      m_x = m_device.createDouble("x", false, 0.0);
-      m_y = m_device.createDouble("y", false, 0.0);
-      m_rot = m_device.createDouble("rot", false, 0.0);
-    }
-  }
-
-  /**
-   * Set the robot pose from a Pose object.
-   *
-   * @param pose 2D pose
-   */
-  public void setRobotPose(Pose2d pose) {
-    if (m_device != null) {
-      Translation2d translation = pose.getTranslation();
-      m_x.set(translation.getX());
-      m_y.set(translation.getY());
-      m_rot.set(pose.getRotation().getDegrees());
-    } else {
-      m_pose = pose;
-    }
-  }
-
-  /**
-   * Set the robot pose from x, y, and rotation.
-   *
-   * @param xMeters X location, in meters
-   * @param yMeters Y location, in meters
-   * @param rotation rotation
-   */
-  public void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) {
-    if (m_device != null) {
-      m_x.set(xMeters);
-      m_y.set(yMeters);
-      m_rot.set(rotation.getDegrees());
-    } else {
-      m_pose = new Pose2d(xMeters, yMeters, rotation);
-    }
-  }
-
-  /**
-   * Get the robot pose.
-   *
-   * @return 2D pose
-   */
-  public Pose2d getRobotPose() {
-    if (m_device != null) {
-      return new Pose2d(m_x.get(), m_y.get(), Rotation2d.fromDegrees(m_rot.get()));
-    } else {
-      return m_pose;
-    }
-  }
-
-  private Pose2d m_pose;
-
-  @SuppressWarnings("MemberName")
-  private final SimDevice m_device;
-
-  @SuppressWarnings("MemberName")
-  private SimDouble m_x;
-
-  @SuppressWarnings("MemberName")
-  private SimDouble m_y;
-
-  private SimDouble m_rot;
-}

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

@@ -0,0 +1,123 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+
+/**
+ * 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 Sendable {
+  /**
+   * Constructor.
+   */
+  public Field2d() {
+    FieldObject2d obj = new FieldObject2d("Robot");
+    obj.setPose(new Pose2d());
+    m_objects.add(obj);
+  }
+
+  /**
+   * 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 xMeters X location, in meters
+   * @param yMeters Y location, in meters
+   * @param rotation rotation
+   */
+  @SuppressWarnings("ParameterName")
+  public synchronized void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) {
+    m_objects.get(0).setPose(xMeters, yMeters, rotation);
+  }
+
+  /**
+   * Get the robot pose.
+   *
+   * @return 2D pose
+   */
+  public synchronized Pose2d getRobotPose() {
+    return m_objects.get(0).getPose();
+  }
+
+  /**
+   * Get or create a field object.
+   *
+   * @return Field object
+   */
+  public synchronized FieldObject2d getObject(String name) {
+    for (FieldObject2d obj : m_objects) {
+      if (obj.m_name == name) {
+        return obj;
+      }
+    }
+    FieldObject2d obj = new FieldObject2d(name);
+    m_objects.add(obj);
+    if (m_table != null) {
+      synchronized (obj) {
+        obj.m_entry = m_table.getEntry(name);
+      }
+    }
+    return obj;
+  }
+
+  /**
+   * Get the robot object.
+   *
+   * @return Field object for robot
+   */
+  public synchronized FieldObject2d getRobotObject() {
+    return m_objects.get(0);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Field2d");
+    m_table = builder.getTable();
+
+    synchronized (this) {
+      for (FieldObject2d obj : m_objects) {
+        synchronized (obj) {
+          obj.m_entry = m_table.getEntry(obj.m_name);
+          obj.updateEntry(true);
+        }
+      }
+    }
+  }
+
+  private NetworkTable m_table;
+  private final List<FieldObject2d> m_objects = new ArrayList<>();
+}

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

@@ -0,0 +1,153 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import java.util.ArrayList;
+import java.util.List;
+
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+
+/**
+ * Game field object on a Field2d.
+ */
+public class FieldObject2d {
+  /**
+   * Package-local constructor.
+   *
+   * @param name name
+   */
+  FieldObject2d(String name) {
+    m_name = name;
+  }
+
+  /**
+   * 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 xMeters X location, in meters
+   * @param yMeters Y location, in meters
+   * @param rotation rotation
+   */
+  @SuppressWarnings("ParameterName")
+  public synchronized void setPose(double xMeters, double yMeters, Rotation2d rotation) {
+    setPose(new Pose2d(xMeters, yMeters, rotation));
+  }
+
+  /**
+   * Get the pose.
+   *
+   * @return 2D pose
+   */
+  public synchronized Pose2d getPose() {
+    updateFromEntry();
+    if (m_poses.isEmpty()) {
+      return new Pose2d();
+    }
+    return m_poses.get(0);
+  }
+
+  /**
+   * Set multiple poses from an 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();
+    for (Pose2d pose : poses) {
+      m_poses.add(pose);
+    }
+    updateEntry();
+  }
+
+  /**
+   * Set multiple poses from an 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();
+    for (Pose2d pose : poses) {
+      m_poses.add(pose);
+    }
+    updateEntry();
+  }
+
+  /**
+   * Get multiple poses.
+   *
+   * @return list of 2D poses
+   */
+  public synchronized List<Pose2d> getPoses() {
+    updateFromEntry();
+    return new ArrayList<Pose2d>(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.setDefaultDoubleArray(arr);
+    } else {
+      m_entry.setDoubleArray(arr);
+    }
+  }
+
+  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
+  private synchronized void updateFromEntry() {
+    if (m_entry == null) {
+      return;
+    }
+
+    double[] arr = m_entry.getDoubleArray((double[]) null);
+    if (arr == null) {
+      return;
+    }
+
+    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;
+  NetworkTableEntry m_entry;
+  private List<Pose2d> m_poses;
+}

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java

@@ -25,7 +25,8 @@ import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.wpilibj.simulation.EncoderSim;
 import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
 import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
-import edu.wpi.first.wpilibj.simulation.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
@@ -64,8 +65,7 @@ public class DriveSubsystem extends SubsystemBase {
   public DifferentialDrivetrainSim m_drivetrainSimulator;
   private EncoderSim m_leftEncoderSim;
   private EncoderSim m_rightEncoderSim;
-  // The Field2d class simulates the field in the sim GUI. Note that we can have only one
-  // instance!
+  // The Field2d class shows the field in the sim GUI
   private Field2d m_fieldSim;
   private SimDouble m_gyroAngleSim;
 
@@ -125,6 +125,7 @@ public class DriveSubsystem extends SubsystemBase {
     m_gyroAngleSim.set(-m_drivetrainSimulator.getHeading().getDegrees());
 
     m_fieldSim.setRobotPose(getPose());
+    SmartDashboard.putData("Field", m_fieldSim);
   }
 
   /**