Clean up new C++ commands (#2027)

- Remove use of std::set. The only place std::set was actually used was in ParallelRaceGroup,
but this was of minimal utility as ParallelRaceGroup checked for duplicate subsystem
requirements, so it would be very unusual to end up with duplicate commands
in any case; replaced it with a vector.
- Remove use of std::unordered_map except for SelectCommand. Replaced with vector.
- Use pImpl idiom for CommandScheduler
- Minimize include files (remove unnecessary ones)
- Reformat include file order for consistency

wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h

@@ -15,14 +15,14 @@
 #include <memory>
 #include <unordered_map>
 #include <utility>
+#include <type_traits>
 #include <vector>
 
-#include "CommandBase.h"
-#include "CommandGroupBase.h"
-#include "PrintCommand.h"
+#include "frc2/command/CommandBase.h"
+#include "frc2/command/CommandGroupBase.h"
+#include "frc2/command/PrintCommand.h"
 
 namespace frc2 {
-template <typename Key>
 /**
  * Runs one of a selection of commands, either using a selector and a key to
  * command mapping, or a supplier that returns the command directly at runtime.
@@ -39,6 +39,7 @@ template <typename Key>
  * <p>As a rule, CommandGroups require the union of the requirements of their
  * component commands.
  */
+template <typename Key>
 class SelectCommand : public CommandHelper<CommandBase, SelectCommand<Key>> {
  public:
   /**