[glass] Field2d enhancements (#3234)

- Add raw support for pose lists > 255/3 in length
- Improve drag selection, especially with closely overlapping objects
- Drag selection of corner also highlights center of object with smaller circle
- Multiple styles (box, line, closed line, track)
- Configurable line and arrow settings (color, weight)
- Add tooltip for object name, index, x, y, rotation
- Context menu for pose edit/add/remove
- View/edit in feet or inches as well as meters
- Configurable object selectability

Implementation: use vector of Pose2d internally, use units

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

@@ -8,6 +8,8 @@ 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;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
 import java.util.ArrayList;
 import java.util.List;
 
@@ -101,20 +103,39 @@ public class FieldObject2d {
       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 (m_poses.size() < (255 / 3)) {
+      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);
+      if (setDefault) {
+        m_entry.setDefaultDoubleArray(arr);
+      } else {
+        m_entry.setDoubleArray(arr);
+      }
     } else {
-      m_entry.setDoubleArray(arr);
+      // send as raw array of doubles if too big for NT array
+      ByteBuffer output = ByteBuffer.allocate(m_poses.size() * 3 * 8);
+      output.order(ByteOrder.BIG_ENDIAN);
+
+      for (Pose2d pose : m_poses) {
+        Translation2d translation = pose.getTranslation();
+        output.putDouble(translation.getX());
+        output.putDouble(translation.getY());
+        output.putDouble(pose.getRotation().getDegrees());
+      }
+
+      if (setDefault) {
+        m_entry.setDefaultRaw(output.array());
+      } else {
+        m_entry.forceSetRaw(output.array());
+      }
     }
   }
 
@@ -125,17 +146,36 @@ public class FieldObject2d {
     }
 
     double[] arr = m_entry.getDoubleArray((double[]) null);
-    if (arr == null) {
-      return;
-    }
+    if (arr != null) {
+      if ((arr.length % 3) != 0) {
+        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])));
+      }
+    } else {
+      // read as raw array of doubles
+      byte[] data = m_entry.getRaw((byte[]) null);
+      if (data == null) {
+        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])));
+      // must be triples of doubles
+      if ((data.length % (3 * 8)) != 0) {
+        return;
+      }
+      ByteBuffer input = ByteBuffer.wrap(data);
+      input.order(ByteOrder.BIG_ENDIAN);
+
+      m_poses.clear();
+      for (int i = 0; i < (data.length / (3 * 8)); i++) {
+        double x = input.getDouble();
+        double y = input.getDouble();
+        double rot = input.getDouble();
+        m_poses.add(new Pose2d(x, y, Rotation2d.fromDegrees(rot)));
+      }
     }
   }