001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.wpilibj.smartdashboard;
006
007import edu.wpi.first.math.geometry.Pose2d;
008import edu.wpi.first.math.geometry.Rotation2d;
009import edu.wpi.first.math.geometry.Translation2d;
010import edu.wpi.first.math.trajectory.Trajectory;
011import edu.wpi.first.networktables.NetworkTableEntry;
012import java.nio.ByteBuffer;
013import java.nio.ByteOrder;
014import java.util.ArrayList;
015import java.util.List;
016
017/** Game field object on a Field2d. */
018public class FieldObject2d {
019  /**
020   * Package-local constructor.
021   *
022   * @param name name
023   */
024  FieldObject2d(String name) {
025    m_name = name;
026  }
027
028  /**
029   * Set the pose from a Pose object.
030   *
031   * @param pose 2D pose
032   */
033  public synchronized void setPose(Pose2d pose) {
034    setPoses(pose);
035  }
036
037  /**
038   * Set the pose from x, y, and rotation.
039   *
040   * @param xMeters X location, in meters
041   * @param yMeters Y location, in meters
042   * @param rotation rotation
043   */
044  @SuppressWarnings("ParameterName")
045  public synchronized void setPose(double xMeters, double yMeters, Rotation2d rotation) {
046    setPose(new Pose2d(xMeters, yMeters, rotation));
047  }
048
049  /**
050   * Get the pose.
051   *
052   * @return 2D pose
053   */
054  public synchronized Pose2d getPose() {
055    updateFromEntry();
056    if (m_poses.isEmpty()) {
057      return new Pose2d();
058    }
059    return m_poses.get(0);
060  }
061
062  /**
063   * Set multiple poses from an list of Pose objects. The total number of poses is limited to 85.
064   *
065   * @param poses list of 2D poses
066   */
067  public synchronized void setPoses(List<Pose2d> poses) {
068    m_poses.clear();
069    for (Pose2d pose : poses) {
070      m_poses.add(pose);
071    }
072    updateEntry();
073  }
074
075  /**
076   * Set multiple poses from an list of Pose objects. The total number of poses is limited to 85.
077   *
078   * @param poses list of 2D poses
079   */
080  public synchronized void setPoses(Pose2d... poses) {
081    m_poses.clear();
082    for (Pose2d pose : poses) {
083      m_poses.add(pose);
084    }
085    updateEntry();
086  }
087
088  /**
089   * Sets poses from a trajectory.
090   *
091   * @param trajectory The trajectory from which the poses should be added.
092   */
093  public synchronized void setTrajectory(Trajectory trajectory) {
094    m_poses.clear();
095    for (Trajectory.State state : trajectory.getStates()) {
096      m_poses.add(state.poseMeters);
097    }
098    updateEntry();
099  }
100
101  /**
102   * Get multiple poses.
103   *
104   * @return list of 2D poses
105   */
106  public synchronized List<Pose2d> getPoses() {
107    updateFromEntry();
108    return new ArrayList<Pose2d>(m_poses);
109  }
110
111  void updateEntry() {
112    updateEntry(false);
113  }
114
115  synchronized void updateEntry(boolean setDefault) {
116    if (m_entry == null) {
117      return;
118    }
119
120    if (m_poses.size() < (255 / 3)) {
121      double[] arr = new double[m_poses.size() * 3];
122      int ndx = 0;
123      for (Pose2d pose : m_poses) {
124        Translation2d translation = pose.getTranslation();
125        arr[ndx + 0] = translation.getX();
126        arr[ndx + 1] = translation.getY();
127        arr[ndx + 2] = pose.getRotation().getDegrees();
128        ndx += 3;
129      }
130
131      if (setDefault) {
132        m_entry.setDefaultDoubleArray(arr);
133      } else {
134        m_entry.setDoubleArray(arr);
135      }
136    } else {
137      // send as raw array of doubles if too big for NT array
138      ByteBuffer output = ByteBuffer.allocate(m_poses.size() * 3 * 8);
139      output.order(ByteOrder.BIG_ENDIAN);
140
141      for (Pose2d pose : m_poses) {
142        Translation2d translation = pose.getTranslation();
143        output.putDouble(translation.getX());
144        output.putDouble(translation.getY());
145        output.putDouble(pose.getRotation().getDegrees());
146      }
147
148      if (setDefault) {
149        m_entry.setDefaultRaw(output.array());
150      } else {
151        m_entry.forceSetRaw(output.array());
152      }
153    }
154  }
155
156  private synchronized void updateFromEntry() {
157    if (m_entry == null) {
158      return;
159    }
160
161    double[] arr = m_entry.getDoubleArray((double[]) null);
162    if (arr != null) {
163      if ((arr.length % 3) != 0) {
164        return;
165      }
166
167      m_poses.clear();
168      for (int i = 0; i < arr.length; i += 3) {
169        m_poses.add(new Pose2d(arr[i], arr[i + 1], Rotation2d.fromDegrees(arr[i + 2])));
170      }
171    } else {
172      // read as raw array of doubles
173      byte[] data = m_entry.getRaw((byte[]) null);
174      if (data == null) {
175        return;
176      }
177
178      // must be triples of doubles
179      if ((data.length % (3 * 8)) != 0) {
180        return;
181      }
182      ByteBuffer input = ByteBuffer.wrap(data);
183      input.order(ByteOrder.BIG_ENDIAN);
184
185      m_poses.clear();
186      for (int i = 0; i < (data.length / (3 * 8)); i++) {
187        double x = input.getDouble();
188        double y = input.getDouble();
189        double rot = input.getDouble();
190        m_poses.add(new Pose2d(x, y, Rotation2d.fromDegrees(rot)));
191      }
192    }
193  }
194
195  String m_name;
196  NetworkTableEntry m_entry;
197  private final List<Pose2d> m_poses = new ArrayList<>();
198}