Added structs for TrapezoidProfile.State and ExponentialProfile.State

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java

@@ -605,6 +605,37 @@ public WrapperCommand withName(String name) {
     return wrapper;
   }
 
+  /**
+   * Throws an error if a parallel group already shares
+   * one or more requirements with a command
+   * that will be added to it.
+   *
+   * @param toAdd The command that will be added to the parallel group.
+   */
+  protected void ensureDisjointRequirements(Command toAdd) {
+    var sharedRequirements = new HashSet<>(getRequirements());
+    sharedRequirements.retainAll(toAdd.getRequirements());
+    if (!sharedRequirements.isEmpty()) {
+      StringBuilder sharedRequirementsStr = new StringBuilder();
+      boolean first = true;
+      for (Subsystem requirement: sharedRequirements) {
+        if (first) {
+          first = false;
+        } else {
+          sharedRequirementsStr.append(", ");
+        }
+        sharedRequirementsStr.append(requirement.getName());
+      }
+      throw new IllegalArgumentException(
+        String.format(
+          "Command %s could not be added to this parallel group"
+          + " because the subsystems [%s] are already required in this command."
+          + " Multiple commands in a parallel composition cannot require"
+          + " the same subsystems.",
+          toAdd.getName(), sharedRequirementsStr));
+    }
+  }
+
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Command");

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java

@@ -4,7 +4,6 @@
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.Collections;
 import java.util.LinkedHashMap;
 import java.util.Map;
 
@@ -51,10 +50,7 @@ public final void addCommands(Command... commands) {
     CommandScheduler.getInstance().registerComposedCommands(commands);
 
     for (Command command : commands) {
-      if (!Collections.disjoint(command.getRequirements(), getRequirements())) {
-        throw new IllegalArgumentException(
-            "Multiple commands in a parallel composition cannot require the same subsystems");
-      }
+      ensureDisjointRequirements(command);
       m_commands.put(command, false);
       addRequirements(command.getRequirements());
       m_runWhenDisabled &= command.runsWhenDisabled();

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java

@@ -5,7 +5,6 @@
 package edu.wpi.first.wpilibj2.command;
 
 import edu.wpi.first.util.sendable.SendableBuilder;
-import java.util.Collections;
 import java.util.LinkedHashMap;
 import java.util.Map;
 
@@ -81,10 +80,7 @@ public final void addCommands(Command... commands) {
     CommandScheduler.getInstance().registerComposedCommands(commands);
 
     for (Command command : commands) {
-      if (!Collections.disjoint(command.getRequirements(), getRequirements())) {
-        throw new IllegalArgumentException(
-            "Multiple commands in a parallel group cannot require the same subsystems");
-      }
+      ensureDisjointRequirements(command);
       m_commands.put(command, false);
       addRequirements(command.getRequirements());
       m_runWhenDisabled &= command.runsWhenDisabled();

wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java

@@ -4,7 +4,6 @@
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.Collections;
 import java.util.LinkedHashSet;
 import java.util.Set;
 
@@ -52,10 +51,7 @@ public final void addCommands(Command... commands) {
     CommandScheduler.getInstance().registerComposedCommands(commands);
 
     for (Command command : commands) {
-      if (!Collections.disjoint(command.getRequirements(), getRequirements())) {
-        throw new IllegalArgumentException(
-            "Multiple commands in a parallel composition cannot require the same subsystems");
-      }
+      ensureDisjointRequirements(command);
       m_commands.add(command);
       addRequirements(command.getRequirements());
       m_runWhenDisabled &= command.runsWhenDisabled();

wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp

@@ -189,6 +189,31 @@ std::optional<std::string> Command::GetPreviousCompositionSite() const {
   return m_previousComposition;
 }
 
+void Command::EnsureDisjointRequirements(Command* toAdd) {
+  std::string sharedRequirementsStr = "";
+  bool hasSharedRequirements = false;
+  auto&& requirementsToAdd = toAdd->GetRequirements();
+  for (auto&& requirement : this->GetRequirements()) {
+    if (requirementsToAdd.find(requirement) != requirementsToAdd.end()) {
+      if (!hasSharedRequirements) {
+        hasSharedRequirements = true;  // ensures formatting like "a, b, c"
+      } else {
+        sharedRequirementsStr.append(", ");
+      }
+      sharedRequirementsStr.append(requirement->GetName());
+    }
+  }
+  if (hasSharedRequirements) {
+    throw FRC_MakeError(
+        frc::err::CommandIllegalUse,
+        "Command {} could not be added to this Parallel Group"
+        " because the subsystems [{}] are already required in this command."
+        " Multiple commands in a parallel composition cannot require the "
+        "same subsystems.",
+        toAdd->GetName(), sharedRequirementsStr);
+  }
+}
+
 void Command::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Command");
   builder.AddStringProperty(".name", [this] { return GetName(); }, nullptr);
@@ -220,14 +245,3 @@ void Command::InitSendable(wpi::SendableBuilder& builder) {
   builder.AddBooleanProperty(
       "runsWhenDisabled", [this] { return RunsWhenDisabled(); }, nullptr);
 }
-
-namespace frc2 {
-bool RequirementsDisjoint(Command* first, Command* second) {
-  bool disjoint = true;
-  auto&& requirements = second->GetRequirements();
-  for (auto&& requirement : first->GetRequirements()) {
-    disjoint &= requirements.find(requirement) == requirements.end();
-  }
-  return disjoint;
-}
-}  // namespace frc2

wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp

@@ -75,19 +75,14 @@ void ParallelCommandGroup::AddCommands(
   }
 
   for (auto&& command : commands) {
-    if (RequirementsDisjoint(this, command.get())) {
-      command->SetComposed(true);
-      AddRequirements(command->GetRequirements());
-      m_runWhenDisabled &= command->RunsWhenDisabled();
-      if (command->GetInterruptionBehavior() ==
-          Command::InterruptionBehavior::kCancelSelf) {
-        m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf;
-      }
-      m_commands.emplace_back(std::move(command), false);
-    } else {
-      throw FRC_MakeError(frc::err::CommandIllegalUse,
-                          "Multiple commands in a parallel group cannot "
-                          "require the same subsystems");
+    EnsureDisjointRequirements(command.get());
+    command->SetComposed(true);
+    AddRequirements(command->GetRequirements());
+    m_runWhenDisabled &= command->RunsWhenDisabled();
+    if (command->GetInterruptionBehavior() ==
+        Command::InterruptionBehavior::kCancelSelf) {
+      m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf;
     }
+    m_commands.emplace_back(std::move(command), false);
   }
 }

wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp

@@ -75,20 +75,15 @@ void ParallelDeadlineGroup::AddCommands(
   }
 
   for (auto&& command : commands) {
-    if (RequirementsDisjoint(this, command.get())) {
-      command->SetComposed(true);
-      AddRequirements(command->GetRequirements());
-      m_runWhenDisabled &= command->RunsWhenDisabled();
-      if (command->GetInterruptionBehavior() ==
-          Command::InterruptionBehavior::kCancelSelf) {
-        m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf;
-      }
-      m_commands.emplace_back(std::move(command), false);
-    } else {
-      throw FRC_MakeError(frc::err::CommandIllegalUse,
-                          "Multiple commands in a parallel group cannot "
-                          "require the same subsystems");
+    EnsureDisjointRequirements(command.get());
+    command->SetComposed(true);
+    AddRequirements(command->GetRequirements());
+    m_runWhenDisabled &= command->RunsWhenDisabled();
+    if (command->GetInterruptionBehavior() ==
+        Command::InterruptionBehavior::kCancelSelf) {
+      m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf;
     }
+    m_commands.emplace_back(std::move(command), false);
   }
 }
 

wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp

@@ -62,19 +62,14 @@ void ParallelRaceGroup::AddCommands(
   }
 
   for (auto&& command : commands) {
-    if (RequirementsDisjoint(this, command.get())) {
-      command->SetComposed(true);
-      AddRequirements(command->GetRequirements());
-      m_runWhenDisabled &= command->RunsWhenDisabled();
-      if (command->GetInterruptionBehavior() ==
-          Command::InterruptionBehavior::kCancelSelf) {
-        m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf;
-      }
-      m_commands.emplace_back(std::move(command));
-    } else {
-      throw FRC_MakeError(frc::err::CommandIllegalUse,
-                          "Multiple commands in a parallel group cannot "
-                          "require the same subsystems");
+    EnsureDisjointRequirements(command.get());
+    command->SetComposed(true);
+    AddRequirements(command->GetRequirements());
+    m_runWhenDisabled &= command->RunsWhenDisabled();
+    if (command->GetInterruptionBehavior() ==
+        Command::InterruptionBehavior::kCancelSelf) {
+      m_interruptBehavior = Command::InterruptionBehavior::kCancelSelf;
     }
+    m_commands.emplace_back(std::move(command));
   }
 }

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

@@ -479,19 +479,21 @@ class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
  protected:
   Command();
 
+  /**
+   * Throws an error if a parallel group already shares
+   * one or more requirements with a command
+   * that will be added to it.
+   *
+   * @param parallelGroup The parallel group command.
+   * @param toAdd The command that will be added to the parallel group.
+   */
+  void EnsureDisjointRequirements(Command* toAdd);
+
  private:
   /// Requirements set.
   wpi::SmallSet<Subsystem*, 4> m_requirements;
 
   std::optional<std::string> m_previousComposition;
 };
 
-/**
- * Checks if two commands have disjoint requirement sets.
- *
- * @param first The first command to check.
- * @param second The second command to check.
- * @return False if first and second share a requirement.
- */
-bool RequirementsDisjoint(Command* first, Command* second);
 }  // namespace frc2

wpimath/src/main/java/edu/wpi/first/math/trajectory/ExponentialProfile.java

@@ -4,6 +4,10 @@
 
 package edu.wpi.first.math.trajectory;
 
+import edu.wpi.first.math.trajectory.struct.ExponentialProfileStateStruct;
+import edu.wpi.first.math.trajectory.struct.ExponentialProfileTimingStruct;
+import edu.wpi.first.util.struct.StructSerializable;
+
 import java.util.Objects;
 
 /**
@@ -40,7 +44,10 @@ public class ExponentialProfile {
   private final Constraints m_constraints;
 
   /** Profile timing. */
-  public static class ProfileTiming {
+  public static class ProfileTiming implements StructSerializable {
+    /** The struct used for serializing this class. */
+    public static final ExponentialProfileTimingStruct struct = new ExponentialProfileTimingStruct();
+
     /** Profile inflection time. */
     public final double inflectionTime;
 
@@ -53,7 +60,7 @@ public static class ProfileTiming {
      * @param inflectionTime Profile inflection time.
      * @param totalTime Total profile time.
      */
-    protected ProfileTiming(double inflectionTime, double totalTime) {
+    public ProfileTiming(double inflectionTime, double totalTime) {
       this.inflectionTime = inflectionTime;
       this.totalTime = totalTime;
     }
@@ -128,7 +135,10 @@ public static Constraints fromStateSpace(double maxInput, double A, double B) {
   }
 
   /** Profile state. */
-  public static class State {
+  public static class State implements StructSerializable {
+    /** The struct used for serializing this class. */
+    public static final ExponentialProfileStateStruct struct = new ExponentialProfileStateStruct();
+
     /** The position at this state. */
     public double position;
 

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java

@@ -6,6 +6,9 @@
 
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.trajectory.struct.TrapezoidProfileStateStruct;
+import edu.wpi.first.util.struct.StructSerializable;
+
 import java.util.Objects;
 
 /**
@@ -75,7 +78,10 @@ public Constraints(double maxVelocity, double maxAcceleration) {
   }
 
   /** Profile state. */
-  public static class State {
+  public static class State implements StructSerializable {
+    /** The struct used for serializing this class. */
+    public static final TrapezoidProfileStateStruct struct = new TrapezoidProfileStateStruct();
+
     /** The position at this state. */
     public double position;
 

wpimath/src/main/java/edu/wpi/first/math/trajectory/struct/ExponentialProfileStateStruct.java

@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.struct;
+
+import edu.wpi.first.math.trajectory.ExponentialProfile;
+import edu.wpi.first.util.struct.Struct;
+
+import java.nio.ByteBuffer;
+
+public class ExponentialProfileStateStruct implements Struct<ExponentialProfile.State> {
+	@Override
+	public Class<ExponentialProfile.State> getTypeClass() {
+		return ExponentialProfile.State.class;
+	}
+
+	@Override
+	public String getTypeName() {
+		return "ExponentialProfileState";
+	}
+
+	@Override
+	public int getSize() {
+		return kSizeDouble * 2;
+	}
+
+	@Override
+	public String getSchema() {
+		return "double position;double velocity";
+	}
+
+	@Override
+	public ExponentialProfile.State unpack(ByteBuffer bb) {
+		return new ExponentialProfile.State(bb.getDouble(), bb.getDouble());
+	}
+
+	@Override
+	public void pack(ByteBuffer bb, ExponentialProfile.State value) {
+		bb.putDouble(value.position);
+		bb.putDouble(value.velocity);
+	}
+}