[wpilib] Move Field2d to SmartDashboard

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