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