[wpilib] Move Field2d to SmartDashboard

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]}}};
+  }
+}