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.networktables.NTSendable;
010import edu.wpi.first.networktables.NTSendableBuilder;
011import edu.wpi.first.networktables.NetworkTable;
012import edu.wpi.first.util.sendable.SendableRegistry;
013import java.util.ArrayList;
014import java.util.List;
015
016/**
017 * 2D representation of game field for dashboards.
018 *
019 * <p>An object's pose is the location shown on the dashboard view. Note that for the robot, this
020 * may or may not match the internal odometry. For example, the robot is shown at a particular
021 * starting location, the pose in this class would represent the actual location on the field, but
022 * the robot's internal state might have a 0,0,0 pose (unless it's initialized to something
023 * different).
024 *
025 * <p>As the user is able to edit the pose, code performing updates should get the robot pose,
026 * transform it as appropriate (e.g. based on wheel odometry), and set the new pose.
027 *
028 * <p>This class provides methods to set the robot pose, but other objects can also be shown by
029 * using the getObject() function. Other objects can also have multiple poses (which will show the
030 * object at multiple locations).
031 */
032public class Field2d implements NTSendable, AutoCloseable {
033  /** Constructor. */
034  public Field2d() {
035    FieldObject2d obj = new FieldObject2d("Robot");
036    obj.setPose(new Pose2d());
037    m_objects.add(obj);
038    SendableRegistry.add(this, "Field");
039  }
040
041  @Override
042  public void close() {
043    for (FieldObject2d obj : m_objects) {
044      obj.close();
045    }
046  }
047
048  /**
049   * Set the robot pose from a Pose object.
050   *
051   * @param pose 2D pose
052   */
053  public synchronized void setRobotPose(Pose2d pose) {
054    m_objects.get(0).setPose(pose);
055  }
056
057  /**
058   * Set the robot pose from x, y, and rotation.
059   *
060   * @param xMeters X location, in meters
061   * @param yMeters Y location, in meters
062   * @param rotation rotation
063   */
064  public synchronized void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) {
065    m_objects.get(0).setPose(xMeters, yMeters, rotation);
066  }
067
068  /**
069   * Get the robot pose.
070   *
071   * @return 2D pose
072   */
073  public synchronized Pose2d getRobotPose() {
074    return m_objects.get(0).getPose();
075  }
076
077  /**
078   * Get or create a field object.
079   *
080   * @param name The field object's name.
081   * @return Field object
082   */
083  public synchronized FieldObject2d getObject(String name) {
084    for (FieldObject2d obj : m_objects) {
085      if (obj.m_name.equals(name)) {
086        return obj;
087      }
088    }
089    FieldObject2d obj = new FieldObject2d(name);
090    m_objects.add(obj);
091    if (m_table != null) {
092      synchronized (obj) {
093        obj.m_entry = m_table.getDoubleArrayTopic(name).getEntry(new double[] {});
094      }
095    }
096    return obj;
097  }
098
099  /**
100   * Get the robot object.
101   *
102   * @return Field object for robot
103   */
104  public synchronized FieldObject2d getRobotObject() {
105    return m_objects.get(0);
106  }
107
108  @Override
109  public void initSendable(NTSendableBuilder builder) {
110    builder.setSmartDashboardType("Field2d");
111
112    synchronized (this) {
113      m_table = builder.getTable();
114      for (FieldObject2d obj : m_objects) {
115        synchronized (obj) {
116          obj.m_entry = m_table.getDoubleArrayTopic(obj.m_name).getEntry(new double[] {});
117          obj.updateEntry(true);
118        }
119      }
120    }
121  }
122
123  private NetworkTable m_table;
124  private final List<FieldObject2d> m_objects = new ArrayList<>();
125}