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.networktables.DoubleArrayPublisher;
008import edu.wpi.first.networktables.NTSendable;
009import edu.wpi.first.networktables.NTSendableBuilder;
010import edu.wpi.first.networktables.NetworkTable;
011import edu.wpi.first.networktables.StringPublisher;
012import edu.wpi.first.wpilibj.util.Color8Bit;
013import java.util.HashMap;
014import java.util.Map;
015import java.util.Map.Entry;
016
017/**
018 * Visual 2D representation of arms, elevators, and general mechanisms through a node-based API.
019 *
020 * <p>A Mechanism2d object is published and contains at least one root node. A root is the anchor
021 * point of other nodes (such as ligaments). Other nodes are recursively appended based on other
022 * nodes.
023 *
024 * @see MechanismObject2d
025 * @see MechanismLigament2d
026 * @see MechanismRoot2d
027 */
028public final class Mechanism2d implements NTSendable, AutoCloseable {
029  private NetworkTable m_table;
030  private final Map<String, MechanismRoot2d> m_roots;
031  private final double[] m_dims = new double[2];
032  private String m_color;
033  private DoubleArrayPublisher m_dimsPub;
034  private StringPublisher m_colorPub;
035
036  /**
037   * Create a new Mechanism2d with the given dimensions and default color (dark blue).
038   *
039   * <p>The dimensions represent the canvas that all the nodes are drawn on.
040   *
041   * @param width the width
042   * @param height the height
043   */
044  public Mechanism2d(double width, double height) {
045    this(width, height, new Color8Bit(0, 0, 32));
046  }
047
048  /**
049   * Create a new Mechanism2d with the given dimensions.
050   *
051   * <p>The dimensions represent the canvas that all the nodes are drawn on.
052   *
053   * @param width the width
054   * @param height the height
055   * @param backgroundColor the background color. Defaults to dark blue.
056   */
057  public Mechanism2d(double width, double height, Color8Bit backgroundColor) {
058    m_roots = new HashMap<>();
059    m_dims[0] = width;
060    m_dims[1] = height;
061    setBackgroundColor(backgroundColor);
062  }
063
064  @Override
065  public void close() {
066    if (m_dimsPub != null) {
067      m_dimsPub.close();
068    }
069    if (m_colorPub != null) {
070      m_colorPub.close();
071    }
072    for (MechanismRoot2d root : m_roots.values()) {
073      root.close();
074    }
075  }
076
077  /**
078   * Get or create a root in this Mechanism2d with the given name and position.
079   *
080   * <p>If a root with the given name already exists, the given x and y coordinates are not used.
081   *
082   * @param name the root name
083   * @param x the root x coordinate
084   * @param y the root y coordinate
085   * @return a new root joint object, or the existing one with the given name.
086   */
087  public synchronized MechanismRoot2d getRoot(String name, double x, double y) {
088    var existing = m_roots.get(name);
089    if (existing != null) {
090      return existing;
091    }
092
093    var root = new MechanismRoot2d(name, x, y);
094    m_roots.put(name, root);
095    if (m_table != null) {
096      root.update(m_table.getSubTable(name));
097    }
098    return root;
099  }
100
101  /**
102   * Set the Mechanism2d background color.
103   *
104   * @param color the new color
105   */
106  public synchronized void setBackgroundColor(Color8Bit color) {
107    m_color = color.toHexString();
108    if (m_colorPub != null) {
109      m_colorPub.set(m_color);
110    }
111  }
112
113  @Override
114  public void initSendable(NTSendableBuilder builder) {
115    builder.setSmartDashboardType("Mechanism2d");
116    synchronized (this) {
117      m_table = builder.getTable();
118      if (m_dimsPub != null) {
119        m_dimsPub.close();
120      }
121      m_dimsPub = m_table.getDoubleArrayTopic("dims").publish();
122      m_dimsPub.set(m_dims);
123      if (m_colorPub != null) {
124        m_colorPub.close();
125      }
126      m_colorPub = m_table.getStringTopic("backgroundColor").publish();
127      m_colorPub.set(m_color);
128      for (Entry<String, MechanismRoot2d> entry : m_roots.entrySet()) {
129        String name = entry.getKey();
130        MechanismRoot2d root = entry.getValue();
131        synchronized (root) {
132          root.update(m_table.getSubTable(name));
133        }
134      }
135    }
136  }
137}