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 static edu.wpi.first.units.Units.Meters;
008
009import edu.wpi.first.math.geometry.Pose2d;
010import edu.wpi.first.math.geometry.Rotation2d;
011import edu.wpi.first.math.geometry.Translation2d;
012import edu.wpi.first.math.trajectory.Trajectory;
013import edu.wpi.first.networktables.DoubleArrayEntry;
014import edu.wpi.first.units.measure.Distance;
015import java.util.ArrayList;
016import java.util.Collections;
017import java.util.List;
018
019/** Game field object on a Field2d. */
020public class FieldObject2d implements AutoCloseable {
021  /**
022   * Package-local constructor.
023   *
024   * @param name name
025   */
026  FieldObject2d(String name) {
027    m_name = name;
028  }
029
030  @Override
031  public void close() {
032    if (m_entry != null) {
033      m_entry.close();
034    }
035  }
036
037  /**
038   * Set the pose from a Pose object.
039   *
040   * @param pose 2D pose
041   */
042  public synchronized void setPose(Pose2d pose) {
043    setPoses(pose);
044  }
045
046  /**
047   * Set the pose from x, y, and rotation.
048   *
049   * @param xMeters X location, in meters
050   * @param yMeters Y location, in meters
051   * @param rotation rotation
052   */
053  public synchronized void setPose(double xMeters, double yMeters, Rotation2d rotation) {
054    setPose(new Pose2d(xMeters, yMeters, rotation));
055  }
056
057  /**
058   * Set the pose from x, y, and rotation.
059   *
060   * @param x X location
061   * @param y Y location
062   * @param rotation rotation
063   */
064  public synchronized void setPose(Distance x, Distance y, Rotation2d rotation) {
065    setPose(new Pose2d(x.in(Meters), y.in(Meters), rotation));
066  }
067
068  /**
069   * Get the pose.
070   *
071   * @return 2D pose
072   */
073  public synchronized Pose2d getPose() {
074    updateFromEntry();
075    if (m_poses.isEmpty()) {
076      return Pose2d.kZero;
077    }
078    return m_poses.get(0);
079  }
080
081  /**
082   * Set multiple poses from a list of Pose objects. The total number of poses is limited to 85.
083   *
084   * @param poses list of 2D poses
085   */
086  public synchronized void setPoses(List<Pose2d> poses) {
087    m_poses.clear();
088    m_poses.addAll(poses);
089    updateEntry();
090  }
091
092  /**
093   * Set multiple poses from a list of Pose objects. The total number of poses is limited to 85.
094   *
095   * @param poses list of 2D poses
096   */
097  public synchronized void setPoses(Pose2d... poses) {
098    m_poses.clear();
099    Collections.addAll(m_poses, poses);
100    updateEntry();
101  }
102
103  /**
104   * Sets poses from a trajectory.
105   *
106   * @param trajectory The trajectory from which the poses should be added.
107   */
108  public synchronized void setTrajectory(Trajectory trajectory) {
109    m_poses.clear();
110    for (Trajectory.State state : trajectory.getStates()) {
111      m_poses.add(state.poseMeters);
112    }
113    updateEntry();
114  }
115
116  /**
117   * Get multiple poses.
118   *
119   * @return list of 2D poses
120   */
121  public synchronized List<Pose2d> getPoses() {
122    updateFromEntry();
123    return new ArrayList<>(m_poses);
124  }
125
126  void updateEntry() {
127    updateEntry(false);
128  }
129
130  synchronized void updateEntry(boolean setDefault) {
131    if (m_entry == null) {
132      return;
133    }
134
135    double[] arr = new double[m_poses.size() * 3];
136    int ndx = 0;
137    for (Pose2d pose : m_poses) {
138      Translation2d translation = pose.getTranslation();
139      arr[ndx + 0] = translation.getX();
140      arr[ndx + 1] = translation.getY();
141      arr[ndx + 2] = pose.getRotation().getDegrees();
142      ndx += 3;
143    }
144
145    if (setDefault) {
146      m_entry.setDefault(arr);
147    } else {
148      m_entry.set(arr);
149    }
150  }
151
152  private synchronized void updateFromEntry() {
153    if (m_entry == null) {
154      return;
155    }
156
157    double[] arr = m_entry.get(null);
158    if (arr != null) {
159      if ((arr.length % 3) != 0) {
160        return;
161      }
162
163      m_poses.clear();
164      for (int i = 0; i < arr.length; i += 3) {
165        m_poses.add(new Pose2d(arr[i], arr[i + 1], Rotation2d.fromDegrees(arr[i + 2])));
166      }
167    }
168  }
169
170  String m_name;
171  DoubleArrayEntry m_entry;
172  private final List<Pose2d> m_poses = new ArrayList<>();
173}