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.util.ErrorMessages.requireNonNullParam;
008
009import edu.wpi.first.networktables.IntegerPublisher;
010import edu.wpi.first.networktables.IntegerTopic;
011import edu.wpi.first.networktables.NTSendable;
012import edu.wpi.first.networktables.NTSendableBuilder;
013import edu.wpi.first.networktables.StringPublisher;
014import edu.wpi.first.networktables.StringTopic;
015import edu.wpi.first.util.sendable.SendableRegistry;
016import java.util.ArrayList;
017import java.util.LinkedHashMap;
018import java.util.List;
019import java.util.Map;
020import java.util.concurrent.atomic.AtomicInteger;
021import java.util.concurrent.locks.ReentrantLock;
022
023/**
024 * The {@link SendableChooser} class is a useful tool for presenting a selection of options to the
025 * {@link SmartDashboard}.
026 *
027 * <p>For instance, you may wish to be able to select between multiple autonomous modes. You can do
028 * this by putting every possible Command you want to run as an autonomous into a {@link
029 * SendableChooser} and then put it into the {@link SmartDashboard} to have a list of options appear
030 * on the laptop. Once autonomous starts, simply ask the {@link SendableChooser} what the selected
031 * value is.
032 *
033 * @param <V> The type of the values to be stored
034 */
035public class SendableChooser<V> implements NTSendable, AutoCloseable {
036  /** The key for the default value. */
037  private static final String DEFAULT = "default";
038  /** The key for the selected option. */
039  private static final String SELECTED = "selected";
040  /** The key for the active option. */
041  private static final String ACTIVE = "active";
042  /** The key for the option array. */
043  private static final String OPTIONS = "options";
044  /** The key for the instance number. */
045  private static final String INSTANCE = ".instance";
046  /** A map linking strings to the objects they represent. */
047  private final Map<String, V> m_map = new LinkedHashMap<>();
048
049  private String m_defaultChoice = "";
050  private final int m_instance;
051  private static final AtomicInteger s_instances = new AtomicInteger();
052
053  /** Instantiates a {@link SendableChooser}. */
054  public SendableChooser() {
055    m_instance = s_instances.getAndIncrement();
056    SendableRegistry.add(this, "SendableChooser", m_instance);
057  }
058
059  @Override
060  public void close() {
061    SendableRegistry.remove(this);
062    m_mutex.lock();
063    try {
064      for (StringPublisher pub : m_activePubs) {
065        pub.close();
066      }
067    } finally {
068      m_mutex.unlock();
069    }
070  }
071
072  /**
073   * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the
074   * object will appear as the given name.
075   *
076   * @param name the name of the option
077   * @param object the option
078   */
079  public void addOption(String name, V object) {
080    m_map.put(name, object);
081  }
082
083  /**
084   * Adds the given object to the list of options and marks it as the default. Functionally, this is
085   * very close to {@link #addOption(String, Object)} except that it will use this as the default
086   * option if none other is explicitly selected.
087   *
088   * @param name the name of the option
089   * @param object the option
090   */
091  public void setDefaultOption(String name, V object) {
092    requireNonNullParam(name, "name", "setDefaultOption");
093
094    m_defaultChoice = name;
095    addOption(name, object);
096  }
097
098  /**
099   * Returns the selected option. If there is none selected, it will return the default. If there is
100   * none selected and no default, then it will return {@code null}.
101   *
102   * @return the option selected
103   */
104  public V getSelected() {
105    m_mutex.lock();
106    try {
107      if (m_selected != null) {
108        return m_map.get(m_selected);
109      } else {
110        return m_map.get(m_defaultChoice);
111      }
112    } finally {
113      m_mutex.unlock();
114    }
115  }
116
117  private String m_selected;
118  private final List<StringPublisher> m_activePubs = new ArrayList<>();
119  private final ReentrantLock m_mutex = new ReentrantLock();
120
121  @Override
122  public void initSendable(NTSendableBuilder builder) {
123    builder.setSmartDashboardType("String Chooser");
124    IntegerPublisher instancePub = new IntegerTopic(builder.getTopic(INSTANCE)).publish();
125    instancePub.set(m_instance);
126    builder.addCloseable(instancePub);
127    builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null);
128    builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null);
129    builder.addStringProperty(
130        ACTIVE,
131        () -> {
132          m_mutex.lock();
133          try {
134            if (m_selected != null) {
135              return m_selected;
136            } else {
137              return m_defaultChoice;
138            }
139          } finally {
140            m_mutex.unlock();
141          }
142        },
143        null);
144    m_mutex.lock();
145    try {
146      m_activePubs.add(new StringTopic(builder.getTopic(ACTIVE)).publish());
147    } finally {
148      m_mutex.unlock();
149    }
150    builder.addStringProperty(
151        SELECTED,
152        null,
153        val -> {
154          m_mutex.lock();
155          try {
156            m_selected = val;
157            for (StringPublisher pub : m_activePubs) {
158              pub.set(val);
159            }
160          } finally {
161            m_mutex.unlock();
162          }
163        });
164  }
165}