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 {
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  /**
042   * Set the robot pose from a Pose object.
043   *
044   * @param pose 2D pose
045   */
046  public synchronized void setRobotPose(Pose2d pose) {
047    m_objects.get(0).setPose(pose);
048  }
049
050  /**
051   * Set the robot pose from x, y, and rotation.
052   *
053   * @param xMeters X location, in meters
054   * @param yMeters Y location, in meters
055   * @param rotation rotation
056   */
057  @SuppressWarnings("ParameterName")
058  public synchronized void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) {
059    m_objects.get(0).setPose(xMeters, yMeters, rotation);
060  }
061
062  /**
063   * Get the robot pose.
064   *
065   * @return 2D pose
066   */
067  public synchronized Pose2d getRobotPose() {
068    return m_objects.get(0).getPose();
069  }
070
071  /**
072   * Get or create a field object.
073   *
074   * @param name The field object's name.
075   * @return Field object
076   */
077  public synchronized FieldObject2d getObject(String name) {
078    for (FieldObject2d obj : m_objects) {
079      if (obj.m_name.equals(name)) {
080        return obj;
081      }
082    }
083    FieldObject2d obj = new FieldObject2d(name);
084    m_objects.add(obj);
085    if (m_table != null) {
086      synchronized (obj) {
087        obj.m_entry = m_table.getEntry(name);
088      }
089    }
090    return obj;
091  }
092
093  /**
094   * Get the robot object.
095   *
096   * @return Field object for robot
097   */
098  public synchronized FieldObject2d getRobotObject() {
099    return m_objects.get(0);
100  }
101
102  @Override
103  public void initSendable(NTSendableBuilder builder) {
104    builder.setSmartDashboardType("Field2d");
105
106    synchronized (this) {
107      m_table = builder.getTable();
108      for (FieldObject2d obj : m_objects) {
109        synchronized (obj) {
110          obj.m_entry = m_table.getEntry(obj.m_name);
111          obj.updateEntry(true);
112        }
113      }
114    }
115  }
116
117  private NetworkTable m_table;
118  private final List<FieldObject2d> m_objects = new ArrayList<>();
119}