[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